diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions new file mode 100644 index 0000000000000..5b140152e7f08 --- /dev/null +++ b/.cppcheck_suppressions @@ -0,0 +1,59 @@ +arrayIndexThenCheck +assignBoolToFloat +checkersReport +constParameterPointer +constParameterReference +constStatement +constVariable +constVariablePointer +constVariableReference +containerOutOfBounds +cstyleCast +ctuOneDefinitionRuleViolation +current_deleted_index +duplicateAssignExpression +duplicateBranch +duplicateBreak +duplicateCondition +duplicateExpression +funcArgNamesDifferent +functionConst +functionStatic +invalidPointerCast +knownConditionTrueFalse +missingInclude +missingIncludeSystem +multiCondition +noConstructor +noExplicitConstructor +noValidConfiguration +obstacle_cruise_planner +passedByValue +preprocessorErrorDirective +redundantAssignment +redundantContinue +redundantIfRemove +redundantInitialization +returnByReference +selfAssignment +shadowArgument +shadowFunction +shadowVariable +stlFindInsert +syntaxError +uninitMemberVar +unknownMacro +unmatchedSuppression +unpreciseMathCall +unreadVariable +unsignedLessThanZero +unusedFunction +unusedScopedObject +unusedStructMember +unusedVariable +useInitializationList +useStlAlgorithm +uselessCallsSubstr +uselessOverride +variableScope +virtualCallInConstructor diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1ad3476333eee..451ade3771f61 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,9 +1,7 @@ ### 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_auto_geometry/** satoshi.ota@tier4.jp -common/autoware_auto_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_auto_tf2/** jit.ray.c@gmail.com 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_point_types/** taichi.higashide@tier4.jp @@ -21,7 +19,6 @@ common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp -common/mission_planner_rviz_plugin/** isamu.takagi@tier4.jp common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @@ -29,27 +26,18 @@ common/path_distance_calculator/** isamu.takagi@tier4.jp common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp -common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp -common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp -common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp -common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp -common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp -common/tier4_screen_capture_rviz_plugin/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp -common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp -common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp +common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp khalil@leodrive.ai common/tier4_system_rviz_plugin/** koji.minoda@tier4.jp -common/tier4_target_object_type_rviz_plugin/** takamasa.horibe@tier4.jp common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -156,13 +144,16 @@ 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_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_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_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_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@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/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_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_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 @@ -179,14 +170,11 @@ 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/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/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_template_module/** daniel.sanchez@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_virtual_traffic_light_module/** kosuke.takeuchi@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 @@ -229,7 +217,6 @@ simulator/fault_injection/** keisuke.shima@tier4.jp simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp -system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai system/bluetooth_monitor/** fumihito.ito@tier4.jp system/component_state_monitor/** isamu.takagi@tier4.jp system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp diff --git a/.github/workflows/build-and-test-arm64.yaml b/.github/workflows/build-and-test-arm64.yaml index a5e00496cc50a..c9a4b46874e18 100644 --- a/.github/workflows/build-and-test-arm64.yaml +++ b/.github/workflows/build-and-test-arm64.yaml @@ -19,7 +19,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository diff --git a/.github/workflows/build-and-test-differential-arm64.yaml b/.github/workflows/build-and-test-differential-arm64.yaml index bd422af46bc4d..d6e49eaabc5bd 100644 --- a/.github/workflows/build-and-test-differential-arm64.yaml +++ b/.github/workflows/build-and-test-differential-arm64.yaml @@ -29,7 +29,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 3ad7b6d434f33..fb98d321fde88 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -28,7 +28,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository @@ -77,7 +77,7 @@ jobs: clang-tidy-differential: runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt-cuda + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda steps: - name: Check out repository uses: actions/checkout@v4 diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 520c2cb9983b0..a469e7a33d190 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -21,7 +21,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository diff --git a/.github/workflows/cppcheck-all.yaml b/.github/workflows/cppcheck-all.yaml new file mode 100644 index 0000000000000..db3bd5d259895 --- /dev/null +++ b/.github/workflows/cppcheck-all.yaml @@ -0,0 +1,60 @@ +name: cppcheck-all + +on: + pull_request: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + cppcheck-all: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Run Cppcheck on all files + continue-on-error: true + id: cppcheck + run: | + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --xml . 2> cppcheck-report.xml + shell: bash + + - name: Count errors by error ID and severity + run: | + #!/bin/bash + temp_file=$(mktemp) + grep -oP '(?<=id=")[^"]+" severity="[^"]+' cppcheck-report.xml | sed 's/" severity="/,/g' > "$temp_file" + echo "Error counts by error ID and severity:" + sort "$temp_file" | uniq -c + rm "$temp_file" + shell: bash + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.xml + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml new file mode 100644 index 0000000000000..914abd7df86ea --- /dev/null +++ b/.github/workflows/cppcheck-differential.yaml @@ -0,0 +1,65 @@ +name: cppcheck-differential + +on: + pull_request: + +jobs: + cppcheck-differential: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Get changed files + id: changed-files + run: | + git fetch origin ${{ github.base_ref }} --depth=1 + git diff --name-only FETCH_HEAD ${{ github.sha }} > changed_files.txt + cat changed_files.txt + + - name: Run Cppcheck on changed files + continue-on-error: true + id: cppcheck + run: | + files=$(cat changed_files.txt | grep -E '\.(cpp|hpp)$' || true) + if [ -n "$files" ]; then + echo "Running Cppcheck on changed files: $files" + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions $files 2> cppcheck-report.txt + else + echo "No C++ files changed." + touch cppcheck-report.txt + fi + shell: bash + + - name: Show cppcheck-report result + run: | + cat cppcheck-report.txt + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.txt + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/dco.yaml b/.github/workflows/dco.yaml new file mode 100644 index 0000000000000..db7ace467c658 --- /dev/null +++ b/.github/workflows/dco.yaml @@ -0,0 +1,21 @@ +name: DCO +# ref: https://github.com/anchore/syft/pull/2926/files +on: + pull_request: +jobs: + dco: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Setup Python 3.x + uses: actions/setup-python@v5 + with: + python-version: 3.x + + - name: Check DCO + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + run: | + pip3 install -U dco-check + dco-check --verbose --exclude-pattern 'pre-commit-ci\[bot\]@users\.noreply\.github\.com' diff --git a/build_depends.repos b/build_depends.repos index 32854cc34e362..a8f431f97df25 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -20,10 +20,6 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git version: main - core/external/autoware_auto_msgs: - type: git - url: https://github.com/tier4/autoware_auto_msgs.git - version: tier4/main # universe universe/external/tier4_autoware_msgs: type: git diff --git a/common/.pages b/common/.pages new file mode 100644 index 0000000000000..38cffcb1dc027 --- /dev/null +++ b/common/.pages @@ -0,0 +1,57 @@ +nav: + - 'Introduction': common + - 'Testing Libraries': + - 'autoware_testing': common/autoware_testing/design/autoware_testing-design + - 'fake_test_node': common/fake_test_node/design/fake_test_node-design + - 'Common Libraries': + - 'autoware_auto_common': + - 'comparisons': common/autoware_auto_common/design/comparisons + - 'autoware_point_types': common/autoware_point_types + - 'Cuda Utils': common/cuda_utils + - 'Geography Utils': common/geography_utils + - 'Global Parameter Loader': common/global_parameter_loader/Readme + - 'Glog Component': common/glog_component + - 'grid_map_utils': common/grid_map_utils + - 'interpolation': common/interpolation + - 'Kalman Filter': common/kalman_filter + - 'Motion Utils': common/motion_utils + - 'Vehicle Utils': common/motion_utils/docs/vehicle/vehicle + - 'Object Recognition Utils': common/object_recognition_utils + - 'OSQP Interface': common/osqp_interface/design/osqp_interface-design + - 'Perception Utils': common/perception_utils + - 'QP Interface': common/qp_interface/design/qp_interface-design + - 'Signal Processing': + - 'Introduction': common/signal_processing + - 'Butterworth Filter': common/signal_processing/documentation/ButterworthFilter + - 'TensorRT Common': common/tensorrt_common + - 'tier4_autoware_utils': common/tier4_autoware_utils + - 'traffic_light_utils': common/traffic_light_utils + - 'TVM Utility': + - 'Introduction': common/tvm_utility + - 'YOLOv2 Tiny Example': common/tvm_utility/tvm-utility-yolo-v2-tiny-tests + - 'RVIZ2 Plugins': + - 'autoware_perception_rviz_plugin': common/autoware_perception_rviz_plugin + - 'autoware_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin + - 'autoware_mission_details_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin + - 'bag_time_manager_rviz_plugin': common/bag_time_manager_rviz_plugin + - 'polar_grid': common/polar_grid/Readme + - 'tier4_adapi_rviz_plugin': common/tier4_adapi_rviz_plugin + - 'tier4_api_utils': common/tier4_api_utils + - 'tier4_camera_view_rviz_plugin': common/tier4_camera_view_rviz_plugin + - 'tier4_datetime_rviz_plugin': common/tier4_datetime_rviz_plugin + - 'tier4_localization_rviz_plugin': common/tier4_localization_rviz_plugin + - 'tier4_perception_rviz_plugin': common/tier4_perception_rviz_plugin + - 'tier4_planning_rviz_plugin': common/tier4_planning_rviz_plugin + - 'tier4_state_rviz_plugin': common/tier4_state_rviz_plugin + - 'tier4_system_rviz_plugin': common/tier4_system_rviz_plugin + - 'tier4_traffic_light_rviz_plugin': common/tier4_traffic_light_rviz_plugin + - 'tier4_vehicle_rviz_plugin': common/tier4_vehicle_rviz_plugin + - 'traffic_light_recognition_marker_publisher': common/traffic_light_recognition_marker_publisher/Readme + - 'Node': + - 'Goal Distance Calculator': common/goal_distance_calculator/Readme + - 'Path Distance Calculator': common/path_distance_calculator/Readme + - 'Others': + - 'autoware_ad_api_specs': common/autoware_ad_api_specs + - 'component_interface_specs': common/component_interface_specs + - 'component_interface_tools': common/component_interface_tools + - 'component_interface_utils': common/component_interface_utils diff --git a/common/README.md b/common/README.md new file mode 100644 index 0000000000000..95b8973a66b2b --- /dev/null +++ b/common/README.md @@ -0,0 +1,16 @@ +# Common + +## Getting Started + +The Autoware.Universe Common folder consists of common and testing libraries that are used by other Autoware components, as well as useful plugins for visualization in RVIZ2. + +!!! note + + In addition to the ones listed in this folder, users can also have a look at some of the add-ons in the `autoware_tools/common` documentation [page](https://autowarefoundation.github.io/autoware_tools/main/common/mission_planner_rviz_plugin/). + +## Highlights + +Some of the commonly used libraries are: + +1. `tier4_autoware_utils` +2. `motion_utils` diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp new file mode 100644 index 0000000000000..09144c1d8ff50 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp @@ -0,0 +1,36 @@ +// Copyright 2024 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. + +#ifndef AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ +#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ + +#include + +#include + +namespace autoware_ad_api::system +{ + +struct Heartbeat +{ + using Message = autoware_adapi_v1_msgs::msg::Heartbeat; + static constexpr char name[] = "/api/system/heartbeat"; + static constexpr size_t depth = 10; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::system + +#endif // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ diff --git a/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp index d3bda57b3374f..cc2fb0e41c372 100644 --- a/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp @@ -36,7 +36,7 @@ using TimeStamp = builtin_interfaces::msg::Time; /// \brief Helper class to check existence of header file in compile time: /// https://stackoverflow.com/a/16000226/2325407 -template +template struct HasHeader : std::false_type { }; @@ -48,60 +48,60 @@ struct HasHeader : std::true_type /////////// Template declarations -/// Get frame id from message. nullptr_t is used to prevent template ambiguity on +/// Get frame id from message. std::nullptr_t is used to prevent template ambiguity on /// SFINAE specializations. Provide a default value on specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template const std::string & get_frame_id(const T & msg) noexcept; -/// Get a reference to the frame id from message. nullptr_t is used to prevent +/// Get a reference to the frame id from message. std::nullptr_t is used to prevent /// template ambiguity on SFINAE specializations. Provide a default value on /// specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template std::string & get_frame_id(T & msg) noexcept; -/// Get stamp from message. nullptr_t is used to prevent template ambiguity on +/// Get stamp from message. std::nullptr_t is used to prevent template ambiguity on /// SFINAE specializations. Provide a default value on specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template const TimeStamp & get_stamp(const T & msg) noexcept; -/// Get a reference to the stamp from message. nullptr_t is used to prevent +/// Get a reference to the stamp from message. std::nullptr_t is used to prevent /// template ambiguity on SFINAE specializations. Provide a default value on /// specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template TimeStamp & get_stamp(T & msg) noexcept; /////////////// Default specializations for message types that contain a header. -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> const std::string & get_frame_id(const T & msg) noexcept { return msg.header.frame_id; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> std::string & get_frame_id(T & msg) noexcept { return msg.header.frame_id; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> TimeStamp & get_stamp(T & msg) noexcept { return msg.header.stamp; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> TimeStamp get_stamp(const T & msg) noexcept { return msg.header.stamp; diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt deleted file mode 100644 index ee819b7cd797c..0000000000000 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_geometry) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - include/autoware_auto_geometry/spatial_hash.hpp - include/autoware_auto_geometry/intersection.hpp - include/autoware_auto_geometry/spatial_hash_config.hpp - src/spatial_hash.cpp - src/bounding_box.cpp -) - -if(BUILD_TESTING) - set(GEOMETRY_GTEST geometry_gtest) - set(GEOMETRY_SRC test/src/test_geometry.cpp - test/src/test_convex_hull.cpp - test/src/test_hull_pockets.cpp - test/src/test_interval.cpp - test/src/lookup_table.cpp - test/src/test_area.cpp - test/src/test_common_2d.cpp - test/src/test_intersection.cpp - ) - ament_add_ros_isolated_gtest(${GEOMETRY_GTEST} ${GEOMETRY_SRC}) - target_compile_options(${GEOMETRY_GTEST} PRIVATE -Wno-conversion -Wno-sign-conversion) - target_include_directories(${GEOMETRY_GTEST} PRIVATE "test/include" "include") - ament_target_dependencies(${GEOMETRY_GTEST} - "autoware_auto_common" - "autoware_auto_geometry_msgs" - "autoware_auto_planning_msgs" - "autoware_auto_vehicle_msgs" - "geometry_msgs" - "osrf_testing_tools_cpp") - target_link_libraries(${GEOMETRY_GTEST} ${PROJECT_NAME}) -endif() - -ament_auto_package() diff --git a/common/autoware_auto_geometry/design/interval.md b/common/autoware_auto_geometry/design/interval.md deleted file mode 100644 index 4fe65ff8e0310..0000000000000 --- a/common/autoware_auto_geometry/design/interval.md +++ /dev/null @@ -1,111 +0,0 @@ -# Interval - -The interval is a standard 1D real-valued interval. -The class implements a representation and operations on the interval type and guarantees interval validity on construction. -Basic operations and accessors are implemented, as well as other common operations. -See 'Example Usage' below. - -## Target use cases - -- Range or containment checks. - The interval class simplifies code that involves checking membership of a value to a range, or intersecting two ranges. - It also provides consistent behavior and consistent handling of edge cases. - -## Properties - -- **empty**: An empty interval is equivalent to an empty set. - It contains no elements. - It is a valid interval, but because it is empty, the notion of measure (length) is undefined; the measure of an empty interval is _not_ zero. - The implementation represents the measure of an empty interval with `NaN`. -- **zero measure**: An interval with zero measure is an interval whose bounds are exactly equal. - The measure is zero because the interval contains only a single point, and points have zero measure. - However, because it does contain a single element, the interval is _not_ empty. -- **valid**: A valid interval is either empty or has min/max bounds such that (min <= max). On construction, interval objects are guaranteed to be valid. - An attempt to construct an invalid interval results in a runtime_error exception being thrown. -- **pseudo-immutable**: Once constructed the only way to change the value of an interval is to overwrite it with a new one; an existing object cannot be modified. - -## Conventions - -- All operations on interval objects are defined as static class methods on the interval class. - This is a functional-style of programming that basically turns the class into a namespace that grants functions access to private member variables of the object they operate on. - -## Assumptions - -- The interval is only intended for floating point types. - This is enforced via static assertion. -- The constructor for non-empty intervals takes two arguments 'min' and 'max', and they must be ordered (i.e., min <= max). - If this assumption is violated, an exception is emitted and construction fails. - -## Example Usage - -```c++ -#include "autoware_auto_geometry/interval.hpp" - -#include - -// using-directive is just for illustration; don't do this in practice -using namespace autoware::common::geometry; - -// bounds for example interval -constexpr auto MIN = 0.0; -constexpr auto MAX = 1.0; - -// -// Try to construct an invalid interval. This will give the following error: -// 'Attempted to construct an invalid interval: {"min": 1.0, "max": 0.0}' -// - -try { - const auto i = Interval_d(MAX, MIN); -} catch (const std::runtime_error& e) { - std::cerr << e.what(); -} - -// -// Construct a double precision interval from 0 to 1 -// - -const auto i = Interval_d(MIN, MAX); - -// -// Test accessors and properties -// - -std::cout << Interval_d::min(i) << " " << Interval_d::max(i) << "\n"; -// Prints: 0.0 1.0 - -std::cout << Interval_d::empty(i) << " " << Interval_d::length(i) << "\n"; -// Prints: false 1.0 - -std::cout << Interval_d::contains(i, 0.3) << "\n"; -// Prints: true - -std::cout << Interval_d::is_subset_eq(Interval_d(0.2, 0.4), i) << "\n"; -// Prints: true - -// -// Test operations. -// - -std::cout << Interval_d::intersect(i, Interval(-1.0, 0.3)) << "\n"; -// Prints: {"min": 0.0, "max": 0.3} - -std::cout << Interval_d::project_to_interval(i, 0.5) << " " - << Interval_d::project_to_interval(i, -1.3) << "\n"; -// Prints: 0.5 0.0 - -// -// Distinguish empty/zero measure -// - -const auto i_empty = Interval(); -const auto i_zero_length = Interval(0.0, 0.0); - -std::cout << Interval_d::empty(i_empty) << " " - << Interval_d::empty(i_zero_length) << "\n"; -// Prints: true false - -std::cout << Interval_d::zero_measure(i_empty) << " " - << Interval_d::zero_measure(i_zero_length) << "\n"; -// Prints: false false -``` diff --git a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md deleted file mode 100644 index f651c218bc80d..0000000000000 --- a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md +++ /dev/null @@ -1,51 +0,0 @@ -# 2D Convex Polygon Intersection - -Two convex polygon's intersection can be visualized on the image below as the blue area: - - - -## Purpose / Use cases - -Computing the intersection between two polygons can be useful in many applications of scene -understanding. It can be used to estimate collision detection, shape alignment, shape -association and in any application that deals with the objects around the perceiving agent. - -## Design - -[\(Livermore, Calif, 1977\)](https://doi.org/10.2172/7309916) mention the following -observations about convex polygon intersection: - -- Intersection of two convex polygons is a convex polygon -- A vertex from a polygon that is contained in the other polygon is a vertex of the intersection - shape. (Vertices A, C, D in the shape above) -- An edge from a polygon that is contained in the other polygon is an edge in the intersection - shape. (edge C-D in the shape above) -- Edge intersections between two polygons are vertices in the intersection shape. (Vertices B, - E in the shape above.) - -### Inner-workings / Algorithms - -With the observation mentioned above, the current algorithm operates in the following way: - -- Compute and find the vertices from each polygon that is contained in the other polygon - (Vertices A, C, D) -- Compute and find the intersection points between each polygon (Vertices B, E) -- Compute the convex hull shaped by these vertices by ordering them CCW. - -### Inputs / Outputs / API - -Inputs: - -- Two iterables that contain vertices of the convex polygons ordered in the CCW direction. - -Outputs: - -- A list of vertices of the intersection shape ordered in the CCW direction. - -## Future Work - -- #1230: Applying efficient algorithms. - -## Related issues - -- #983: Integrate vision detections in object tracker diff --git a/common/autoware_auto_geometry/design/spatial-hash-design.md b/common/autoware_auto_geometry/design/spatial-hash-design.md deleted file mode 100644 index 58eecf3ee841b..0000000000000 --- a/common/autoware_auto_geometry/design/spatial-hash-design.md +++ /dev/null @@ -1,99 +0,0 @@ -# Spatial Hash - -The spatial hash is a data structure designed for efficient fixed-radius near-neighbor queries in -low dimensions. - -The fixed-radius near-neighbors problem is defined as follows: - -`For point p, find all points p' s.t. d(p, p') < r` - -Where in this case `d(p, p')` is euclidean distance, and `r` is the fixed -radius. - -For `n` points with an average of `k` neighbors each, this data structure can -perform `m` near-neighbor queries (to generate lists of near-neighbors for `m` -different points) in `O(mk)` time. - -By contrast, using a k-d tree for successive nearest-neighbor queries results in -a running time of `O(m log n)`. - -The spatial hash works as follows: - -- Each point is assigned to a bin in the predefined bounding area defined by - `x_min/x_max` and `y_min/y_max` -- This can be done by converting x and y position into x and y index - respectively - - For example with the bin containing `x_min` and `y_min` as index `(0, 0)` - - The two (or more) indices can then be converted into a single index -- Once every point of interest has been inserted into the hash, near-neighbor - queries can begin: - - The bin of the reference point is first computed - - For each point in each adjacent bin, perform an explicit distance computation - between said point and the reference point. If the distance is below the given - radius, said point is considered to be a near-neighbor - -Under the hood, an `std::unordered_multimap` is used, where the key is a bin/voxel index. -The bin size was computed to be the same as the lookup distance. - - - -In addition, this data structure can support 2D or 3D queries. This is determined during -configuration, and baked into the data structure via the configuration class. The purpose of -this was to avoid if statements in tight loops. The configuration class specializations themselves -use CRTP (Curiously Recurring Template Patterns) to do "static polymorphism", and avoid -a dispatching call. - -## Performance characterization - -### Time - -Insertion is `O(n)` because lookup time for the underlying hashmap is `O(n)` for -hashmaps. In practice, lookup time for hashmaps and thus insertion time should -be `O(1)`. - -Removing a point is `O(1)` because the current API only supports removal via -direct reference to a node. - -Finding `k` near-neighbors is worst case `O(n)` in the case of an adversarial -example, but in practice `O(k)`. - -### Space - -The module consists of the following components: - -- The internal hashmap is `O(n + n + A * n)`, where `A` is an arbitrary - constant (load factor) -- The other components of the spatial hash are `O(n + n)` - -This results in `O(n)` space complexity. - -## States - -The spatial hash's state is dictated by the status of the underlying unordered_multimap. - -The data structure is wholly configured by a -[config](@ref autoware::common::geometry::spatial_hash::Config) class. The constructor -of the class determines in the data structure accepts strictly 2D or strictly 3D queries. - -## Inputs - -The primary method of introducing data into the data structure is via the -[insert](@ref autoware::common::geometry::spatial_hash::SpatialHashBase::insert) method. - -## Outputs - -The primary method of retrieving data from the data structure is via the -[near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near)\(2D -configuration\) -or [near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near) -\(3D configuration\) method. - -The whole data structure can also be traversed using standard constant iterators. - -## Future Work - -- Performance tuning and optimization - -## Related issues - -- #28: Port to autoware.Auto diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp deleted file mode 100644 index 0f3a68e14d442..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp +++ /dev/null @@ -1,188 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief Common functionality for bounding box computation algorithms - -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include -#include -#include -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief Functions and types for generating enclosing bounding boxes around a set of points -namespace bounding_box -{ -using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; - -/// \brief Computes height of bounding box given a full list of points -/// \param[in] begin The start of the list of points -/// \param[in] end One past the end of the list of points -/// \param[out] box A box for which the z component of centroid, corners, and size gets filled -/// \tparam IT An iterator type, must dereference into a point type with float member z, or -/// appropriate point adapter defined -template -void compute_height(const IT begin, const IT end, BoundingBox & box) -{ - float32_t max_z = -std::numeric_limits::max(); - float32_t min_z = std::numeric_limits::max(); - for (auto it = begin; it != end; ++it) { - const float32_t z = point_adapter::z_(*it); - if (z <= min_z) { - min_z = z; - } - if (z >= max_z) { - max_z = z; - } - } - box.centroid.z = (max_z + min_z) * 0.5F; - for (auto & corner : box.corners) { - corner.z = box.centroid.z; - } - box.size.z = (max_z - min_z) * 0.5F; -} - -/// \brief Computes height of bounding box given a full list of points -/// \param[in] begin Iterator pointing to the start of the range of points -/// \param[in] end Iterator pointing the the end of the range of points -/// \param[out] shape A shape in which vertices z values and height field will be set -/// \tparam IT An iterator type, must dereference into a point type with float member z, or -/// appropriate point adapter defined -template -void compute_height(const IT begin, const IT end, autoware_auto_perception_msgs::msg::Shape & shape) -{ - float32_t max_z = -std::numeric_limits::max(); - float32_t min_z = std::numeric_limits::max(); - for (auto it = begin; it != end; ++it) { - const float32_t z = point_adapter::z_(*it); - if (z <= min_z) { - min_z = z; - } - if (z >= max_z) { - max_z = z; - } - } - for (auto & corner : shape.footprint.points) { - corner.z = min_z; - } - shape.dimensions.z = max_z - min_z; -} - -namespace details -{ -/// Templated alias for getting a type without CV or reference qualification -template -using base_type = typename std::remove_cv::type>::type; - -/// Templated alias for an array of 4 objects of type PointT -template -using Point4 = std::array; - -/// \brief Helper struct for compile time introspection of array size from -/// Ref: https://stackoverflow.com/questions/16866033 -template -struct array_size; -template -struct array_size> -{ - static std::size_t const size = N; -}; -static_assert( - array_size>::size == 4U, - "Bounding box does not have the right number of corners"); - -/// \brief Compute length, width, area of bounding box -/// \param[in] corners Corners of the current bounding box -/// \param[out] ret A point struct used to hold size of box defined by corners -void GEOMETRY_PUBLIC -size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret); -/// \brief Computes centroid and orientation of a box given corners -/// \param[in] corners Array of final corners of bounding box -/// \param[out] box Message to have corners, orientation, and centroid updated -void GEOMETRY_PUBLIC -finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box); - -/// \brief given support points and two orthogonal directions, compute corners of bounding box -/// \tparam PointT Type of a point, must have float members x and y` -/// \tparam IT An iterator dereferenceable into PointT -/// \param[out] corners Gets filled with corner points of bounding box -/// \param[in] supports Iterators referring to support points of current bounding box -/// e.g. bounding box is touching these points -/// \param[in] directions Directions of each edge of the bounding box from each support point -template -void compute_corners( - decltype(BoundingBox::corners) & corners, const Point4 & supports, - const Point4 & directions) -{ - for (uint32_t idx = 0U; idx < corners.size(); ++idx) { - const uint32_t jdx = (idx != 3U) ? (idx + 1U) : 0U; - const auto pt = - intersection_2d(*supports[idx], directions[idx], *supports[jdx], directions[jdx]); - // TODO(c.ho) handle error - point_adapter::xr_(corners[idx]) = point_adapter::x_(pt); - point_adapter::yr_(corners[idx]) = point_adapter::y_(pt); - } -} -// TODO(c.ho) type trait enum base - -/// \brief Copy vertices of the given box into a Shape type -/// \param box Box to be converted -/// \return Shape type filled with box vertices -autoware_auto_perception_msgs::msg::Shape GEOMETRY_PUBLIC make_shape(const BoundingBox & box); - -/// \brief Copy centroid and orientation info of the box into Pose type -/// \param box BoundingBox to be converted -/// \return Pose type filled with centroid and orientation from box -geometry_msgs::msg::Pose GEOMETRY_PUBLIC make_pose(const BoundingBox & box); - -/// \brief Fill DetectedObject type with contents from a BoundingBox type -/// \param box BoundingBox to be converted -/// \return Filled DetectedObject type -autoware_auto_perception_msgs::msg::DetectedObject GEOMETRY_PUBLIC -make_detected_object(const BoundingBox & box); - -/// \brief Transform corners from object-local coordinates using the given centroid and orientation -/// \param shape_msg Msg containing corners in object-local coordinates -/// \param centroid Centroid of the polygon whose corners are defined in shape_msg -/// \param orientation Orientation of the polygon -/// \return corners transformed such that their centroid and orientation correspond to the -/// given inputs -std::vector GEOMETRY_PUBLIC get_transformed_corners( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation); - -} // namespace details -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp deleted file mode 100644 index e0f2e66e87ee5..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp +++ /dev/null @@ -1,247 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements 2D pca on a linked list of points to estimate an oriented -/// bounding box - -// cspell: ignore eigenbox, EIGENBOX -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ - -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ - -/// \brief Simplified 2d covariance matrix -struct Covariance2d -{ - /// \brief Variance in the x direction - float32_t xx; - /// \brief Variance in the y direction - float32_t yy; - /// \brief x-y covariance - float32_t xy; - /// \brief Number of points - std::size_t num_points; -}; // struct Covariance2d - -// cspell: ignore Welford -/// \brief Compute 2d covariance matrix of a list of points using Welford's online algorithm -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \return A 2d covariance matrix for all points in the list -template -Covariance2d covariance_2d(const IT begin, const IT end) -{ - Covariance2d ret{0.0F, 0.0F, 0.0F, 0U}; - float32_t ux = 0.0F; - float32_t uy = 0.0F; - float32_t num_points = 0.0F; - using point_adapter::x_; - using point_adapter::y_; - for (auto it = begin; it != end; ++it) { - ++ret.num_points; - num_points = static_cast(ret.num_points); - const auto & pt = *it; - // update mean x - const float32_t dx = x_(pt) - ux; - ux = ux + (dx / num_points); - // update cov - const float32_t dy = y_(pt) - uy; - ret.xy += (x_(pt) - ux) * (dy); - // update mean y - uy = uy + (dy / num_points); - // update M2 - ret.xx += dx * (x_(pt) - ux); - ret.yy += dy * (y_(pt) - uy); - } - // finalize sample (co-)variance - if (ret.num_points > 1U) { - num_points = num_points - 1.0F; - } - ret.xx /= num_points; - ret.yy /= num_points; - ret.xy /= num_points; - - return ret; -} - -/// \brief Compute eigenvectors and eigenvalues -/// \param[in] cov 2d Covariance matrix -/// \param[out] eig_vec1 First eigenvector -/// \param[out] eig_vec2 Second eigenvector -/// \tparam PointT Point type that has at least float members x and y -/// \return A pair of eigenvalues: The first is the larger eigenvalue -/// \throw std::runtime error if you would get degenerate covariance -template -std::pair eig_2d( - const Covariance2d & cov, PointT & eig_vec1, PointT & eig_vec2) -{ - const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; - const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); - // Add a small fudge to alleviate floating point errors - float32_t disc = ((tr_2 * tr_2) - det) + std::numeric_limits::epsilon(); - if (disc < 0.0F) { - throw std::runtime_error( - "pca_2d: negative discriminant! Should never happen for well formed " - "covariance matrix"); - } - disc = sqrtf(disc); - // compute eigenvalues - const auto ret = std::make_pair(tr_2 + disc, tr_2 - disc); - // compute eigenvectors - using point_adapter::xr_; - using point_adapter::yr_; - // We compare squared value against floating epsilon to make sure that eigen vectors - // are persistent against further calculations. - // (e.g. taking cross product of two eigen vectors) - if (fabsf(cov.xy * cov.xy) > std::numeric_limits::epsilon()) { - xr_(eig_vec1) = cov.xy; - yr_(eig_vec1) = ret.first - cov.xx; - xr_(eig_vec2) = cov.xy; - yr_(eig_vec2) = ret.second - cov.xx; - } else { - if (cov.xx > cov.yy) { - xr_(eig_vec1) = 1.0F; - yr_(eig_vec1) = 0.0F; - xr_(eig_vec2) = 0.0F; - yr_(eig_vec2) = 1.0F; - } else { - xr_(eig_vec1) = 0.0F; - yr_(eig_vec1) = 1.0F; - xr_(eig_vec2) = 1.0F; - yr_(eig_vec2) = 0.0F; - } - } - return ret; -} - -/// \brief Given eigenvectors, compute support (furthest) point in each direction -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \tparam PointT type of a point with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] eig1 First principal component of cluster -/// \param[in] eig2 Second principal component of cluster -/// \param[out] support Array to get filled with extreme points in each of the principal -/// components -/// \return whether or not eig2 is ccw wrt eig1 -template -bool8_t compute_supports( - const IT begin, const IT end, const PointT & eig1, const PointT & eig2, Point4 & support) -{ - const bool8_t ret = cross_2d(eig1, eig2) >= 0.0F; - std::array metrics{ - {-std::numeric_limits::max(), -std::numeric_limits::max(), - std::numeric_limits::max(), std::numeric_limits::max()}}; - for (auto it = begin; it != end; ++it) { - const PointT & pt = *it; - float32_t val = dot_2d(ret ? eig1 : eig2, pt); - if (val > metrics[0U]) { - metrics[0U] = val; - support[0U] = it; - } - if (val < metrics[2U]) { - metrics[2U] = val; - support[2U] = it; - } - val = dot_2d(ret ? eig2 : eig1, pt); - if (val > metrics[1U]) { - metrics[1U] = val; - support[1U] = it; - } - if (val < metrics[3U]) { - metrics[3U] = val; - support[3U] = it; - } - } - return ret; -} - -/// \brief Compute bounding box given a pair of basis directions -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \tparam PointT Point type of the lists, must have float members x and y -/// \param[in] ax1 First basis direction, assumed to be normal to ax2 -/// \param[in] ax2 Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1 -/// \param[in] supports Array of iterators referring to points that are most extreme in each basis -/// direction. Should be result of compute_supports function -/// \return A bounding box based on the basis axes and the support points -template -BoundingBox compute_bounding_box( - const PointT & ax1, const PointT & ax2, const Point4 & supports) -{ - decltype(BoundingBox::corners) corners; - const Point4 directions{{ax1, ax2, minus_2d(ax1), minus_2d(ax2)}}; - compute_corners(corners, supports, directions); - - // build box - BoundingBox bbox; - finalize_box(corners, bbox); - size_2d(corners, bbox.size); - return bbox; -} -} // namespace details - -/// \brief Compute oriented bounding box using PCA. This uses all points in a list, and does not -/// modify the list. The resulting bounding box is not necessarily minimum in any way -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \return An oriented bounding box in x-y. This bounding box has no height information -template -BoundingBox eigenbox_2d(const IT begin, const IT end) -{ - // compute covariance - const details::Covariance2d cov = details::covariance_2d(begin, end); - - // compute eigenvectors - using PointT = details::base_type; - PointT eig1; - PointT eig2; - const auto eig_v = details::eig_2d(cov, eig1, eig2); - - // find extreme points - details::Point4 supports; - const bool8_t is_ccw = details::compute_supports(begin, end, eig1, eig2, supports); - // build box - if (is_ccw) { - std::swap(eig1, eig2); - } - BoundingBox bbox = details::compute_bounding_box(eig1, eig2, supports); - bbox.value = eig_v.first; - - return bbox; -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp deleted file mode 100644 index 07fd6c989cedc..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp +++ /dev/null @@ -1,281 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements 2D pca on a linked list of points to estimate an oriented -/// bounding box - -// cspell: ignore LFIT, lfit -// LFIT means "L-Shape Fitting" -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ - -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ - -/// \brief A representation of the M matrix for the L-fit algorithm -struct LFitWs -{ - /// \brief Number of points in the first partition - std::size_t p; - /// \brief Number of points in the second partition - std::size_t q; - // assume matrix of form: [a b; c d] - /// \brief Sum of x values in first partition - float32_t m12a; - /// \brief Sum of y values in first partition - float32_t m12b; - /// \brief Sum of y values in second partition - float32_t m12c; - /// \brief Negative sum of x values in second partition - float32_t m12d; - // m22 is a symmetric matrix - /// \brief Sum_p x_2 + sum_q y_2 - float32_t m22a; - /// \brief Sum_p x*y - sum_q x*y - float32_t m22b; - /// \brief Sum_p y_2 + sum_x y_2 - float32_t m22d; -}; // struct LFitWs - -/// \brief Initialize M matrix for L-fit algorithm -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \param[out] ws A representation of the M matrix to get initialized -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -template -void init_lfit_ws(const IT begin, const IT end, const std::size_t size, LFitWs & ws) -{ - ws.p = 1UL; - ws.q = size - 1UL; - // init P terms (first partition) - using point_adapter::x_; - using point_adapter::y_; - const auto & pt = *begin; - const float32_t px = x_(pt); - const float32_t py = y_(pt); - // assume matrix of form: [a b; c d] - ws.m12a = px; - ws.m12b = py; - ws.m12c = 0.0F; - ws.m12d = 0.0F; - // m22 is a symmetric matrix - ws.m22a = px * px; - ws.m22b = px * py; - ws.m22d = py * py; - auto it = begin; - ++it; - for (; it != end; ++it) { - const auto & qt = *it; - const float32_t qx = x_(qt); - const float32_t qy = y_(qt); - ws.m12c += qy; - ws.m12d -= qx; - ws.m22a += qy * qy; - ws.m22b -= qx * qy; - ws.m22d += qx * qx; - } -} - -/// \brief Solves the L fit problem for a given M matrix -/// \tparam PointT The point type of the cluster being L-fitted -/// \param[in] ws A representation of the M Matrix -/// \param[out] dir The best fit direction for this partition/M matrix -/// \return The L2 residual for this fit (the score, lower is better) -template -float32_t solve_lfit(const LFitWs & ws, PointT & dir) -{ - const float32_t pi = 1.0F / static_cast(ws.p); - const float32_t qi = 1.0F / static_cast(ws.q); - const Covariance2d M{// matrix of form [x, z; z y] - ws.m22a - (((ws.m12a * ws.m12a) * pi) + ((ws.m12c * ws.m12c) * qi)), - ws.m22d - (((ws.m12b * ws.m12b) * pi) + ((ws.m12d * ws.m12d) * qi)), - ws.m22b - (((ws.m12a * ws.m12b) * pi) + ((ws.m12c * ws.m12d) * qi)), 0UL}; - PointT eig1; - const auto eig_v = eig_2d(M, eig1, dir); - return eig_v.second; -} - -/// \brief Increments L fit M matrix with information in the point -/// \tparam PointT The point type -/// \param[in] pt The point to increment the M matrix with -/// \param[inout] ws A representation of the M matrix -template -void increment_lfit_ws(const PointT & pt, LFitWs & ws) -{ - const float32_t px = point_adapter::x_(pt); - const float32_t py = point_adapter::y_(pt); - ws.m12a += px; - ws.m12b += py; - ws.m12c -= py; - ws.m12d += px; - ws.m22b += 2.0F * px * py; - const float32_t px2y2 = (px - py) * (px + py); - ws.m22a += px2y2; - ws.m22d -= px2y2; -} - -/// \tparam IT An iterator type that should dereference into a point type with float members x and y -template -class LFitCompare -{ -public: - /// \brief Constructor, initializes normal direction - /// \param[in] hint A 2d vector with the direction that will be used to order the - /// point list - explicit LFitCompare(const PointT & hint) - : m_nx(point_adapter::x_(hint)), m_ny(point_adapter::y_(hint)) - { - } - - /// \brief Comparator operation, returns true if the projection of a the tangent line - /// is smaller than the projection of b - /// \param[in] p The first point for comparison - /// \param[in] q The second point for comparison - /// \return True if a has a smaller projection than b on the tangent line - bool8_t operator()(const PointT & p, const PointT & q) const - { - using point_adapter::x_; - using point_adapter::y_; - return ((x_(p) * m_nx) + (y_(p) * m_ny)) < ((x_(q) * m_nx) + (y_(q) * m_ny)); - } - -private: - const float32_t m_nx; - const float32_t m_ny; -}; // class LFitCompare - -/// \brief The main implementation of L-fitting a bounding box to a list of points. -/// Assumes sufficiently valid, large enough, and appropriately ordered point list -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \return A bounding box that minimizes the LFit residual -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -template -BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::size_t size) -{ - // initialize M - LFitWs ws{}; - init_lfit_ws(begin, end, size, ws); - // solve initial problem - details::base_type best_normal; - float32_t min_eig = solve_lfit(ws, best_normal); - // solve subsequent problems - auto it = begin; - ++it; - for (; it != end; ++it) { - // update M - ws.p += 1U; - ws.q -= 1U; - if (ws.q == 0U) { // checks for q = 0 case - break; - } - increment_lfit_ws(*it, ws); - // solve incremented problem - decltype(best_normal) dir; - const float32_t score = solve_lfit(ws, dir); - // update optima - if (score < min_eig) { - min_eig = score; - best_normal = dir; - } - } - // can recover best corner point, but don't care, need to cover all points - const auto inv_norm = 1.0F / norm_2d(best_normal); - if (!std::isnormal(inv_norm)) { - throw std::runtime_error{"LFit: Abnormal norm"}; - } - best_normal = times_2d(best_normal, inv_norm); - auto best_tangent = get_normal(best_normal); - // find extreme points - Point4 supports; - const bool8_t is_ccw = details::compute_supports(begin, end, best_normal, best_tangent, supports); - if (is_ccw) { - std::swap(best_normal, best_tangent); - } - BoundingBox bbox = details::compute_bounding_box(best_normal, best_tangent, supports); - bbox.value = min_eig; - - return bbox; -} -} // namespace details - -/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed -/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation" -/// \return An oriented bounding box in x-y. This bounding box has no height information -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \param[in] hint An iterator pointing to the point whose normal will be used to sort the list -/// \return A pair where the first element is an iterator pointing to the nearest point, and the -/// second element is the size of the list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \throw std::domain_error If the number of points is too few -template -BoundingBox lfit_bounding_box_2d( - const IT begin, const IT end, const PointT hint, const std::size_t size) -{ - if (size <= 1U) { - throw std::domain_error("LFit requires >= 2 points!"); - } - // NOTE: assumes points are in base_link/sensor frame! - // sort along tangent wrt sensor origin - // lint -e522 NOLINT Not a pure function: data structure iterators are pointing to is modified - std::partial_sort(begin, end, end, details::LFitCompare{hint}); - - return details::lfit_bounding_box_2d_impl(begin, end, size); -} - -/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed -/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation". -/// This implementation sorts the list using std::sort -/// \return An oriented bounding box in x-y. This bounding box has no height information -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -template -BoundingBox lfit_bounding_box_2d(const IT begin, const IT end) -{ - // use the principal component as the sorting direction - const auto cov = details::covariance_2d(begin, end); - using PointT = details::base_type; - PointT eig1; - PointT eig2; - (void)details::eig_2d(cov, eig1, eig2); - (void)eig2; - return lfit_bounding_box_2d(begin, end, eig1, cov.num_points); -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp deleted file mode 100644 index fb75384eb07cb..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp +++ /dev/null @@ -1,280 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements the rotating calipers algorithm for minimum oriented bounding boxes - -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/convex_hull.hpp" - -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ -/// \brief Find which (next) edge has smallest angle delta to directions, rotate directions -/// \param[inout] edges Array of edges on polygon after each anchor point (in ccw order). -/// E.g. if anchor point = p_i, edge = P[i+1] - P[i] -/// \param[inout] directions Array of directions of current bounding box (in ccw order) -/// \tparam PointT Point type of the lists, must have float members x and y -/// \return index of array to advance, e.g. the one with the smallest angle between edge and dir -template -uint32_t update_angles(const Point4 & edges, Point4 & directions) -{ - // find smallest angle to next - float32_t best_cos_th = -std::numeric_limits::max(); - float32_t best_edge_dir_mag = 0.0F; - uint32_t advance_idx = 0U; - for (uint32_t idx = 0U; idx < edges.size(); ++idx) { - const float32_t edge_dir_mag = std::max( - norm_2d(edges[idx]) * norm_2d(directions[idx]), std::numeric_limits::epsilon()); - const float32_t cos_th = dot_2d(edges[idx], directions[idx]) / edge_dir_mag; - if (cos_th > best_cos_th) { - best_cos_th = cos_th; - best_edge_dir_mag = edge_dir_mag; - advance_idx = idx; - } - } - - // update directions by smallest angle - const float32_t sin_th = - cross_2d(directions[advance_idx], edges[advance_idx]) / best_edge_dir_mag; - for (uint32_t idx = 0U; idx < edges.size(); ++idx) { - rotate_2d(directions[idx], best_cos_th, sin_th); - } - - return advance_idx; -} - -/// \brief Given support points i, find direction of edge: e = P[i+1] - P[i] -/// \tparam PointT Point type of the lists, must have float members x and y -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] support Array of points that are most extreme in 4 directions for current OBB -/// \param[out] edges An array to be filled with the polygon edges next from support points -template -void init_edges(const IT begin, const IT end, const Point4 & support, Point4 & edges) -{ - for (uint32_t idx = 0U; idx < support.size(); ++idx) { - auto tmp_it = support[idx]; - ++tmp_it; - const PointT & q = (tmp_it == end) ? *begin : *tmp_it; - edges[idx] = minus_2d(q, *support[idx]); - } -} - -/// \brief Scan through list to find support points for bounding box oriented in direction of -/// u = P[1] - P[0] -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[out] support Array that gets filled with pointers to points that are most extreme in -/// each direction aligned with and orthogonal to the first polygon edge. -/// Most extreme = max/min wrt u = P[1]-P[0] (in the dot/cross product sense) -template -void init_bbox(const IT begin, const IT end, Point4 & support) -{ - // compute initial orientation using first two points - auto pt_it = begin; - ++pt_it; - const auto u = minus_2d(*pt_it, *begin); - support[0U] = begin; - std::array metric{ - {-std::numeric_limits::max(), -std::numeric_limits::max(), - std::numeric_limits::max()}}; - // track u * p, fabsf(u x p), and -u * p - for (pt_it = begin; pt_it != end; ++pt_it) { - // check points against orientation for run_ptr - const auto & pt = *pt_it; - // u * p - const float32_t up = dot_2d(u, pt); - if (up > metric[0U]) { - metric[0U] = up; - support[1U] = pt_it; - } - // -u * p - if (up < metric[2U]) { - metric[2U] = up; - support[3U] = pt_it; - } - // u x p - const float32_t uxp = cross_2d(u, pt); - if (uxp > metric[1U]) { - metric[1U] = uxp; - support[2U] = pt_it; - } - } -} -/// \brief Compute the minimum bounding box for a convex hull using the rotating calipers method. -/// This function may possibly also be used for computing the width of a convex hull, as it uses the -/// external supports of a single convex hull. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \param[in] metric_fn A functor determining what measure the bounding box is minimum with respect -/// to -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -/// \tparam MetricF A functor that computes a float measure from the x and y size (width and length) -/// of a bounding box, assumed to be packed in a Point32 message. -/// \throw std::domain_error if the list of points is too small to reasonable generate a bounding -/// box -template -BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF metric_fn) -{ - using PointT = base_type; - // sanity check TODO(c.ho) more checks (up to n = 2?) - if (begin == end) { - throw std::domain_error("Malformed list, size = " + std::to_string(std::distance(begin, end))); - } - // initial scan to get anchor points - Point4 support; - init_bbox(begin, end, support); - // initialize directions accordingly - Point4 directions; - { // I just don't want the temporary variable floating around - auto tmp = support[0U]; - ++tmp; - directions[0U] = minus_2d(*tmp, *support[0U]); - directions[1U] = get_normal(directions[0U]); - directions[2U] = minus_2d(directions[0U]); - directions[3U] = minus_2d(directions[1U]); - } - // initialize edges - Point4 edges; - init_edges(begin, end, support, edges); - // size of initial guess - BoundingBox bbox; - decltype(BoundingBox::corners) best_corners; - compute_corners(best_corners, support, directions); - size_2d(best_corners, bbox.size); - bbox.value = metric_fn(bbox.size); - // rotating calipers step: incrementally advance, update angles, points, compute area - for (auto it = begin; it != end; ++it) { - // find smallest angle to next, update directions - const uint32_t advance_idx = update_angles(edges, directions); - // recompute area - decltype(BoundingBox::corners) corners; - compute_corners(corners, support, directions); - decltype(BoundingBox::size) tmp_size; - size_2d(corners, tmp_size); - const float32_t tmp_value = metric_fn(tmp_size); - // update best if necessary - if (tmp_value < bbox.value) { - // corners: memcpy is fine since I know corners and best_corners are distinct - (void)std::memcpy(&best_corners[0U], &corners[0U], sizeof(corners)); - // size - bbox.size = tmp_size; - bbox.value = tmp_value; - } - // Step to next iteration of calipers - { - // update smallest support - auto next_it = support[advance_idx]; - ++next_it; - const auto run_it = (end == next_it) ? begin : next_it; - support[advance_idx] = run_it; - // update edges - next_it = run_it; - ++next_it; - const PointT & next = (end == next_it) ? (*begin) : (*next_it); - edges[advance_idx] = minus_2d(next, *run_it); - } - } - - finalize_box(best_corners, bbox); - - // TODO(christopher.ho) check if one of the sizes is too small, fuzz corner 1 and 2 - // This doesn't cause any issues now, it shouldn't happen in practice, and even if it did, - // it would probably get smoothed out by prediction. But, this could cause issues with the - // collision detection algorithms (i.e GJK), but probably not. - - return bbox; -} -} // namespace details - -/// \brief Compute the minimum area bounding box given a convex hull of points. -/// This function is exposed in case a user might already have a convex hull via other methods. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \return A minimum area bounding box, value field is the area -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -template -BoundingBox minimum_area_bounding_box(const IT begin, const IT end) -{ - const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t { - return pt.x * pt.y; - }; - return details::rotating_calipers_impl(begin, end, metric_fn); -} - -/// \brief Compute the minimum perimeter bounding box given a convex hull of points -/// This function is exposed in case a user might already have a convex hull via other methods. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -template -BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end) -{ - const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t { - return pt.x + pt.y; - }; - return details::rotating_calipers_impl(begin, end, metric_fn); -} - -/// \brief Compute the minimum area bounding box given an unstructured list of points. -/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and -/// without memory allocation. -/// \param[inout] list A list of points to form a hull around, gets reordered -/// \return A minimum area bounding box, value field is the area -/// \tparam PointT Point type of the lists, must have float members x and y -template -BoundingBox minimum_area_bounding_box(std::list & list) -{ - const auto last = convex_hull(list); - return minimum_area_bounding_box(list.cbegin(), last); -} - -/// \brief Compute the minimum perimeter bounding box given an unstructured list of points -/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and -/// without memory allocation. -/// \param[inout] list A list of points to form a hull around, gets reordered -/// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam PointT Point type of the lists, must have float members x and y -template -BoundingBox minimum_perimeter_bounding_box(std::list & list) -{ - const auto last = convex_hull(list); - return minimum_perimeter_bounding_box(list.cbegin(), last); -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp deleted file mode 100644 index c4c52928ac19a..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief Main header for user-facing bounding box algorithms: functions and types -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ - -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp deleted file mode 100644 index e3c2e58c9f80e..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp +++ /dev/null @@ -1,587 +0,0 @@ -// Copyright 2017-2021 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common functionality for 2D geometry, such as dot products - -#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; - -namespace comparison = autoware::common::helper_functions::comparisons; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \brief Temporary namespace for point adapter methods, for use with nonstandard point types -namespace point_adapter -{ -/// \brief Gets the x value for a point -/// \return The x value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto x_(const PointT & pt) -{ - return pt.x; -} -/// \brief Gets the x value for a TrajectoryPoint message -/// \return The x value of the point -/// \param[in] pt The point -inline auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.x; -} -/// \brief Gets the y value for a point -/// \return The y value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto y_(const PointT & pt) -{ - return pt.y; -} -/// \brief Gets the y value for a TrajectoryPoint message -/// \return The y value of the point -/// \param[in] pt The point -inline auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.y; -} -/// \brief Gets the z value for a point -/// \return The z value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto z_(const PointT & pt) -{ - return pt.z; -} -/// \brief Gets the z value for a TrajectoryPoint message -/// \return The z value of the point -/// \param[in] pt The point -inline auto z_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.z; -} -/// \brief Gets a reference to the x value for a point -/// \return A reference to the x value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & xr_(PointT & pt) -{ - return pt.x; -} -/// \brief Gets a reference to the x value for a TrajectoryPoint -/// \return A reference to the x value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & xr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.x; -} -/// \brief Gets a reference to the y value for a point -/// \return A reference to The y value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & yr_(PointT & pt) -{ - return pt.y; -} -/// \brief Gets a reference to the y value for a TrajectoryPoint -/// \return A reference to the y value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & yr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.y; -} -/// \brief Gets a reference to the z value for a point -/// \return A reference to the z value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & zr_(PointT & pt) -{ - return pt.z; -} -/// \brief Gets a reference to the z value for a TrajectoryPoint -/// \return A reference to the z value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & zr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.z; -} -} // namespace point_adapter - -namespace details -{ -// Return the next iterator, cycling back to the beginning of the list if you hit the end -template -IT circular_next(const IT begin, const IT end, const IT current) noexcept -{ - auto next = std::next(current); - if (end == next) { - next = begin; - } - return next; -} - -} // namespace details - -/// \tparam T1, T2, T3 point type. Must have point adapters defined or have float members x and y -/// \brief compute whether line segment rp is counter clockwise relative to line segment qp -/// \param[in] pt shared point for both line segments -/// \param[in] r point to check if it forms a ccw angle -/// \param[in] q reference point -/// \return whether angle formed is ccw. Three collinear points is considered ccw -template -inline auto ccw(const T1 & pt, const T2 & q, const T3 & r) -{ - using point_adapter::x_; - using point_adapter::y_; - return (((x_(q) - x_(pt)) * (y_(r) - y_(pt))) - ((y_(q) - y_(pt)) * (x_(r) - x_(pt)))) <= 0.0F; -} - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y -/// \brief compute p x q = p1 * q2 - p2 * q1 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 2d cross product -template -inline auto cross_2d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - return (x_(pt) * y_(q)) - (y_(pt) * x_(q)); -} - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y -/// \brief compute p * q = p1 * q1 + p2 * q2 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 2d scalar dot product -template -inline auto dot_2d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - return (x_(pt) * x_(q)) + (y_(pt) * y_(q)); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the 2d difference between two points, p - q -/// \param[in] p The left hand side -/// \param[in] q The right hand side -/// \return A point with the difference in the x and y fields, all other fields are default -/// initialized -template -T minus_2d(const T & p, const T & q) -{ - T r; - using point_adapter::x_; - using point_adapter::y_; - point_adapter::xr_(r) = x_(p) - x_(q); - point_adapter::yr_(r) = y_(p) - y_(q); - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The unary minus or negation operator applied to a single point's 2d fields -/// \param[in] p The left hand side -/// \return A point with the negation in the x and y fields, all other fields are default -/// initialized -template -T minus_2d(const T & p) -{ - T r; - point_adapter::xr_(r) = -point_adapter::x_(p); - point_adapter::yr_(r) = -point_adapter::y_(p); - return r; -} -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The 2d addition operation, p + q -/// \param[in] p The left hand side -/// \param[in] q The right hand side -/// \return A point with the sum in the x and y fields, all other fields are default -/// initialized -template -T plus_2d(const T & p, const T & q) -{ - T r; - using point_adapter::x_; - using point_adapter::y_; - point_adapter::xr_(r) = x_(p) + x_(q); - point_adapter::yr_(r) = y_(p) + y_(q); - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The scalar multiplication operation, p * a -/// \param[in] p The point value -/// \param[in] a The scalar value -/// \return A point with the scaled x and y fields, all other fields are default -/// initialized -template -T times_2d(const T & p, const float32_t a) -{ - T r; - point_adapter::xr_(r) = static_cast(point_adapter::x_(p)) * a; - point_adapter::yr_(r) = static_cast(point_adapter::y_(p)) * a; - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief solve p + t * u = q + s * v -/// Ref: https://stackoverflow.com/questions/563198/ -/// \param[in] pt anchor point of first line -/// \param[in] u direction of first line -/// \param[in] q anchor point of second line -/// \param[in] v direction of second line -/// \return intersection point -/// \throw std::runtime_error if lines are (nearly) collinear or parallel -template -inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v) -{ - const float32_t num = cross_2d(minus_2d(pt, q), u); - float32_t den = cross_2d(v, u); - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - if (fabsf(den) < FEPS) { - if (fabsf(num) < FEPS) { - // collinear case, anything is ok - den = 1.0F; - } else { - // parallel case, no valid output - throw std::runtime_error( - "intersection_2d: no unique solution (either collinear or parallel)"); - } - } - return plus_2d(q, times_2d(v, num / den)); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief rotate point given precomputed sin and cos -/// \param[inout] pt point to rotate -/// \param[in] cos_th precomputed cosine value -/// \param[in] sin_th precomputed sine value -template -inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th) -{ - const float32_t x = point_adapter::x_(pt); - const float32_t y = point_adapter::y_(pt); - point_adapter::xr_(pt) = (cos_th * x) - (sin_th * y); - point_adapter::yr_(pt) = (sin_th * x) + (cos_th * y); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief rotate by radian angle th in z direction with ccw positive -/// \param[in] pt reference point to rotate -/// \param[in] th_rad angle by which to rotate point -/// \return rotated point -template -inline T rotate_2d(const T & pt, const float32_t th_rad) -{ - T q(pt); // It's reasonable to expect a copy constructor - const float32_t s = sinf(th_rad); - const float32_t c = cosf(th_rad); - rotate_2d(q, c, s); - return q; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief compute q s.t. p T q, or p * q = 0 -/// This is the equivalent of a 90 degree ccw rotation -/// \param[in] pt point to get normal point of -/// \return point normal to p (un-normalized) -template -inline T get_normal(const T & pt) -{ - T q(pt); - point_adapter::xr_(q) = -point_adapter::y_(pt); - point_adapter::yr_(q) = point_adapter::x_(pt); - return q; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief get magnitude of x and y components: -/// \param[in] pt point to get magnitude of -/// \return magnitude of x and y components together -template -inline auto norm_2d(const T & pt) -{ - return sqrtf(static_cast(dot_2d(pt, pt))); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the closest point on line segment p-q to point r -/// Based on equations from https://stackoverflow.com/a/1501725 and -/// http://paulbourke.net/geometry/pointlineplane/ -/// \param[in] p First point defining the line segment -/// \param[in] q Second point defining the line segment -/// \param[in] r Reference point to find the closest point to -/// \return Closest point on line segment p-q to point r -template -inline T closest_segment_point_2d(const T & p, const T & q, const T & r) -{ - const T qp = minus_2d(q, p); - const float32_t len2 = static_cast(dot_2d(qp, qp)); - T ret = p; - if (len2 > std::numeric_limits::epsilon()) { - const Interval_f unit_interval(0.0f, 1.0f); - const float32_t val = static_cast(dot_2d(minus_2d(r, p), qp)) / len2; - const float32_t t = Interval_f::clamp_to(unit_interval, val); - ret = plus_2d(p, times_2d(qp, t)); - } - return ret; -} -// -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the closest point on the line going through p-q to point r -// Obtained by simplifying closest_segment_point_2d. -/// \param[in] p First point defining the line -/// \param[in] q Second point defining the line -/// \param[in] r Reference point to find the closest point to -/// \return Closest point on line p-q to point r -/// \throw std::runtime_error if the two points coincide and hence don't uniquely -// define a line -template -inline T closest_line_point_2d(const T & p, const T & q, const T & r) -{ - const T qp = minus_2d(q, p); - const float32_t len2 = dot_2d(qp, qp); - T ret = p; - if (len2 > std::numeric_limits::epsilon()) { - const float32_t t = dot_2d(minus_2d(r, p), qp) / len2; - ret = plus_2d(p, times_2d(qp, t)); - } else { - throw std::runtime_error( - "closet_line_point_2d: line ill-defined because given points coincide"); - } - return ret; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the distance from line segment p-q to point r -/// \param[in] p First point defining the line segment -/// \param[in] q Second point defining the line segment -/// \param[in] r Reference point to find the distance from the line segment to -/// \return Distance from point r to line segment p-q -template -inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r) -{ - const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r); - return norm_2d(pq_r); -} - -/// \brief Make a 2D unit vector given an angle. -/// \tparam T Point type. Must have point adapters defined or have float members x and y -/// \param th Angle in radians -/// \return Unit vector in the direction of the given angle. -template -inline T make_unit_vector2d(float th) -{ - T r; - point_adapter::xr_(r) = std::cos(th); - point_adapter::yr_(r) = std::sin(th); - return r; -} - -/// \brief Compute squared euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x and y -/// \tparam T2 point type. Must have point adapters defined or have float members x and y -/// \param a point 1 -/// \param b point 2 -/// \return squared euclidean distance -template -inline OUT squared_distance_2d(const T1 & a, const T2 & b) -{ - const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); - const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); - return (x * x) + (y * y); -} - -/// \brief Compute euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x and y -/// \tparam T2 point type. Must have point adapters defined or have float members x and y -/// \param a point 1 -/// \param b point 2 -/// \return euclidean distance -template -inline OUT distance_2d(const T1 & a, const T2 & b) -{ - return std::sqrt(squared_distance_2d(a, b)); -} - -/// \brief Check the given point's position relative the infinite line passing -/// from p1 to p2. Logic based on http://geomalgorithms.com/a01-_area.html#isLeft() -/// \tparam T T point type. Must have point adapters defined or have float members x and y -/// \param p1 point 1 laying on the infinite line -/// \param p2 point 2 laying on the infinite line -/// \param q point to be checked against the line -/// \return > 0 for point q left of the line through p1 to p2 -/// = 0 for point q on the line through p1 to p2 -/// < 0 for point q right of the line through p1 to p2 -template -inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T & q) -{ - return cross_2d(minus_2d(p2, p1), minus_2d(q, p1)); -} - -/// Check if all points are ordered in x-y plane (in either clockwise or counter-clockwise -/// direction): This function does not check for convexity -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Beginning of point sequence -/// \param[in] end One past the last of the point sequence -/// \return Whether or not all point triples p_i, p_{i+1}, p_{i+2} are in a particular order. -/// Returns true for collinear points as well -template -bool all_ordered(const IT begin, const IT end) noexcept -{ - // Short circuit: a line or point is always CCW or otherwise ill-defined - if (std::distance(begin, end) <= 2U) { - return true; - } - bool is_first_point_cw = false; - // Can't use std::all_of because that works directly on the values - for (auto line_start = begin; line_start != end; ++line_start) { - const auto line_end = details::circular_next(begin, end, line_start); - const auto query_point = details::circular_next(begin, end, line_end); - // Check if 3 points starting from current point are in clockwise direction - const bool is_cw = comparison::abs_lte( - check_point_position_to_line_2d(*line_start, *line_end, *query_point), 0.0F, 1e-3F); - if (is_cw) { - if (line_start == begin) { - is_first_point_cw = true; - } else { - if (!is_first_point_cw) { - return false; - } - } - } else if (is_first_point_cw) { - return false; - } - } - return true; -} - -/// Compute the area of a convex hull, points are assumed to be ordered (in either CW or CCW) -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Iterator pointing to the beginning of the polygon points -/// \param[in] end Iterator pointing to one past the last of the polygon points -/// \return The area of the polygon, in squared of whatever units your points are in -template -auto area_2d(const IT begin, const IT end) noexcept -{ - using point_adapter::x_; - using point_adapter::y_; - using T = decltype(x_(*begin)); - auto area = T{}; // zero initialization - // use the approach of https://www.mathwords.com/a/area_convex_polygon.htm - for (auto it = begin; it != end; ++it) { - const auto next = details::circular_next(begin, end, it); - area += x_(*it) * y_(*next); - area -= x_(*next) * y_(*it); - } - return std::abs(T{0.5} * area); -} - -/// Compute area of convex hull, throw if points are not ordered (convexity check is not -/// implemented) -/// \throw std::domain_error if points are not ordered either CW or CCW -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Iterator pointing to the beginning of the polygon points -/// \param[in] end Iterator pointing to one past the last of the polygon points -/// \return The area of the polygon, in squared of whatever units your points are in -template -auto area_checked_2d(const IT begin, const IT end) -{ - if (!all_ordered(begin, end)) { - throw std::domain_error{"Cannot compute area: points are not ordered"}; - } - return area_2d(begin, end); -} - -/// \brief Check if the given point is inside or on the edge of the given polygon -/// \tparam IteratorType iterator type. The value pointed to by this must have point adapters -/// defined or have float members x and y -/// \tparam PointType point type. Must have point adapters defined or have float members x and y -/// \param start_it iterator pointing to the first vertex of the polygon -/// \param end_it iterator pointing to the last vertex of the polygon. The vertices should be in -/// order. -/// \param p point to be searched -/// \return True if the point is inside or on the edge of the polygon. False otherwise -template -bool is_point_inside_polygon_2d( - const IteratorType & start_it, const IteratorType & end_it, const PointType & p) -{ - std::int32_t winding_num = 0; - - for (IteratorType it = start_it; it != end_it; ++it) { - auto next_it = std::next(it); - if (next_it == end_it) { - next_it = start_it; - } - if (point_adapter::y_(*it) <= point_adapter::y_(p)) { - // Upward crossing edge - if (point_adapter::y_(*next_it) >= point_adapter::y_(p)) { - if (comparison::abs_gt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { - ++winding_num; - } else { - if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) { - return true; - } - } - } - } else { - // Downward crossing edge - if (point_adapter::y_(*next_it) <= point_adapter::y_(p)) { - if (comparison::abs_lt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { - --winding_num; - } else { - if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) { - return true; - } - } - } - } - } - return winding_num != 0; -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp deleted file mode 100644 index 74cd210dec586..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common functionality for 3D geometry, such as dot products - -#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x, y and z -/// \brief compute p * q = p1 * q1 + p2 * q2 + p3 * 13 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 3d scalar dot product -template -inline auto dot_3d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - using point_adapter::z_; - return (x_(pt) * x_(q)) + (y_(pt) * y_(q) + z_(pt) * z_(q)); -} - -/// \brief Compute 3D squared euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x y and z -/// \tparam T2 point type. Must have point adapters defined or have float members x y and z -/// \param a point 1 -/// \param b point 2 -/// \return squared 3D euclidean distance -template -inline OUT squared_distance_3d(const T1 & a, const T2 & b) -{ - const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); - const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); - const auto z = static_cast(point_adapter::z_(a)) - static_cast(point_adapter::z_(b)); - return (x * x) + (y * y) + (z * z); -} - -/// \brief Compute 3D euclidean distance between two points -/// \tparam T1 point type. Must have point adapters defined or have float members x y and z -/// \tparam T2 point type. Must have point adapters defined or have float members x y and z -/// \param a point 1 -/// \param b point 2 -/// \return 3D euclidean distance -template -inline OUT distance_3d(const T1 & a, const T2 & b) -{ - return std::sqrt(squared_distance_3d(a, b)); -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp deleted file mode 100644 index ae81c55ad6b55..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp +++ /dev/null @@ -1,195 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements the monotone chain algorithm to compute 2D convex hulls on linked -/// lists of points - -#ifndef AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -// lint -e537 NOLINT pclint vs cpplint -#include -// lint -e537 NOLINT pclint vs cpplint -#include -#include -#include - -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief Contains computation geometry functions not intended for the end user to directly use -namespace details -{ - -/// \brief Moves points comprising the lower convex hull from points to hull. -/// \param[inout] points A list of points, assumed to be sorted in lexical order -/// \param[inout] hull An empty list of points, assumed to have same allocator as points -/// (for splice) -/// \tparam PointT The point type for the points list -/// \tparam HullT the point type for the hull list -template -void form_lower_hull(std::list & points, std::list & hull) -{ - auto hull_it = hull.cbegin(); - auto point_it = points.cbegin(); - // This ensures that the points we splice to back to the end of list are not processed - const auto iters = points.size(); - for (auto idx = decltype(iters){0}; idx < iters; ++idx) { - // splice points from tail of hull to tail of list until point from head of list satisfies ccw - bool8_t is_ccw = true; - while ((hull.cbegin() != hull_it) && is_ccw) { - const auto current_hull_it = hull_it; - --hull_it; - is_ccw = ccw(*hull_it, *current_hull_it, *point_it); - if (!is_ccw) { - hull_it = current_hull_it; - break; - } - // return this node to list for consideration in upper hull - points.splice(points.end(), hull, current_hull_it); - } - const auto last_point_it = point_it; - ++point_it; - // Splice head of list to tail of (lower) hull - hull.splice(hull.end(), points, last_point_it); - // point_it has been advanced, hull_it has been advanced (to where point_it was previously) - hull_it = last_point_it; - } - // loop is guaranteed not to underflow because a node can be removed from points AT MOST once - // per loop iteration. The loop is upper bounded to the prior size of the point list -} -/// \brief Moves points comprising the lower convex hull from points to hull. -/// \param[inout] points A list of points, assumed to be sorted in lexical order, and to contain -/// the leftmost point -/// \param[inout] hull A list of points, assumed to have same allocator as points (for splice), -/// and to contain the lower hull (minus the left-most point) -/// \tparam PointT The point type for the points list -/// \tparam HullT the point type for the hull list -template -void form_upper_hull(std::list & points, std::list & hull) -{ - // TODO(c.ho) consider reverse iterators, not sure if they work with splice() - auto hull_it = hull.cend(); - --hull_it; - const auto lower_hull_end = hull_it; - auto point_it = points.cend(); - --point_it; - // This ensures that the points we splice to back to the head of list are not processed - const auto iters = points.size(); - for (auto idx = decltype(iters){0}; idx < iters; ++idx) { - // splice points from tail of hull to head of list until ccw is satisfied with tail of list - bool8_t is_ccw = true; - while ((lower_hull_end != hull_it) && is_ccw) { - const auto current_hull_it = hull_it; - --hull_it; - is_ccw = ccw(*hull_it, *current_hull_it, *point_it); - if (!is_ccw) { - hull_it = current_hull_it; - break; - } - points.splice(points.begin(), hull, current_hull_it); - } - const auto last_point_it = point_it; - --point_it; - // Splice points from tail of lexically ordered point list to tail of hull - hull.splice(hull.end(), points, last_point_it); - hull_it = last_point_it; - } - // loop is guaranteed not to underflow because a node can be removed from points AT MOST once - // per loop iteration. The loop is upper bounded to the prior size of the point list -} - -/// \brief A static memory implementation of convex hull computation. Shuffles points around the -/// deque such that the points of the convex hull of the deque of points are first in the -/// deque, with the internal points following in an unspecified order. -/// The head of the deque will be the point with the smallest x value, with the other -/// points following in a counter-clockwise manner (from a top down view/facing -z direction) -/// \param[inout] list A list of nodes that will be pruned down and reordered into a ccw convex hull -/// \return An iterator pointing to one after the last point contained in the hull -/// \tparam PointT Type of a point, must have x and y float members -template -typename std::list::const_iterator convex_hull_impl(std::list & list) -{ - // Functor that return whether a <= b in the lexical sense (a.x < b.x), sort by y if tied - const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t { - using point_adapter::x_; - using point_adapter::y_; - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - return (fabsf(x_(a) - x_(b)) > FEPS) ? (x_(a) < x_(b)) : (y_(a) < y_(b)); - }; - list.sort(lexical_comparator); - - // Temporary list to store points - std::list tmp_hull_list{list.get_allocator()}; - - // Shuffle lower hull points over to tmp_hull_list - form_lower_hull(list, tmp_hull_list); - - // Resort list since we can't guarantee the order TODO(c.ho) is this true? - list.sort(lexical_comparator); - // splice first hull point to beginning of list to ensure upper hull is properly closed - // This is done after sorting because we know exactly where it should go, and keeping it out of - // sorting reduces complexity somewhat - list.splice(list.begin(), tmp_hull_list, tmp_hull_list.begin()); - - // build upper hull - form_upper_hull(list, tmp_hull_list); - // Move hull to beginning of list, return iterator pointing to one after the convex hull - const auto ret = list.begin(); - // first move left-most point to ensure it is first - auto tmp_it = tmp_hull_list.end(); - --tmp_it; - list.splice(list.begin(), tmp_hull_list, tmp_it); - // Then move remainder of hull points - list.splice(ret, tmp_hull_list); - return ret; -} -} // namespace details - -/// \brief A static memory implementation of convex hull computation. Shuffles points around the -/// deque such that the points of the convex hull of the deque of points are first in the -/// deque, with the internal points following in an unspecified order. -/// -/// The head of the deque will be the point with the smallest x value, with the other -/// points following in a counter-clockwise manner (from a top down view/facing -z -/// direction). If the point list is 3 or smaller, nothing is done (e.g. the ordering result -/// as previously stated does not hold). -/// \param[inout] list A list of nodes that will be reordered into a ccw convex hull -/// \return An iterator pointing to one after the last point contained in the hull -/// \tparam PointT Type of a point, must have x and y float members -template -typename std::list::const_iterator convex_hull(std::list & list) -{ - return (list.size() <= 3U) ? list.end() : details::convex_hull_impl(list); -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp deleted file mode 100644 index cd9fd49f71635..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements an algorithm for getting a list of "pockets" in the convex -/// hull of a non-convex simple polygon. - -#ifndef AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -#include -#include -#include -#include -#include - -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \brief Compute a list of "pockets" of a simple polygon -/// (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make -/// up the difference between the polygon and its convex hull. -/// This currently has a bug: -// * "Rollover" is not properly handled - if a pocket contains the segment from -// the last point in the list to the first point (which is still part of the -// polygon), that does not get added -/// -/// \param[in] polygon_start Start iterator for the simple polygon (has to be on convex hull) -/// \param[in] polygon_end End iterator for the simple polygon -/// \param[in] convex_hull_start Start iterator for the convex hull of the simple polygon -/// \param[in] convex_hull_end End iterator for the convex hull of the simple polygon -/// \return A vector of vectors of the iterator value type. Each inner vector contains the points -/// for one pocket. We return by value instead of as iterator pairs, because it is possible -/// that the edge connecting the final point in the list and the first point in the list is -/// part of a pocket as well, and this is awkward to represent using iterators into the -/// original collection. -/// -/// \tparam Iter1 Iterator to a point type -/// \tparam Iter2 Iterator to a point type (can be the same as Iter1, but does not need to be) -template -typename std::vector::value_type>> hull_pockets( - const Iter1 polygon_start, const Iter1 polygon_end, const Iter2 convex_hull_start, - const Iter2 convex_hull_end) -{ - auto pockets = std::vector::value_type>>{}; - if (std::distance(polygon_start, polygon_end) <= 3) { - return pockets; - } - - // Function to check whether a point is in the convex hull. - const auto in_convex_hull = [convex_hull_start, convex_hull_end](Iter1 p) { - return std::any_of(convex_hull_start, convex_hull_end, [p](auto hull_entry) { - return norm_2d(minus_2d(hull_entry, *p)) < std::numeric_limits::epsilon(); - }); - }; - - // We go through the points of the polygon only once, adding pockets to the list of pockets - // as we detect them. - std::vector::value_type> current_pocket{}; - for (auto it = polygon_start; it != polygon_end; it = std::next(it)) { - if (in_convex_hull(it)) { - if (current_pocket.size() > 1) { - current_pocket.emplace_back(*it); - pockets.push_back(current_pocket); - } - current_pocket.clear(); - current_pocket.emplace_back(*it); - } else { - current_pocket.emplace_back(*it); - } - } - - // At this point, we have reached the end of the polygon, but have not considered the connection - // of the final point back to the first. In case the first point is part of a pocket as well, we - // have some points left in current_pocket, and we add one final pocket that is made up by those - // points plus the first point in the collection. - if (current_pocket.size() > 1) { - current_pocket.push_back(*polygon_start); - pockets.push_back(current_pocket); - } - - return pockets; -} -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp deleted file mode 100644 index 08c70c3a5a6be..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/convex_hull.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -using autoware::common::geometry::closest_line_point_2d; -using autoware::common::geometry::convex_hull; -using autoware::common::geometry::dot_2d; -using autoware::common::geometry::get_normal; -using autoware::common::geometry::minus_2d; -using autoware::common::geometry::norm_2d; -using autoware::common::geometry::times_2d; -using autoware_auto_perception_msgs::msg::BoundingBox; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; - -using Point = geometry_msgs::msg::Point32; - -namespace details -{ - -/// Alias for a std::pair of two points -using Line = std::pair; - -/// \tparam Iter1 Iterator over point-types that must have point adapters -// defined or have float members x and y -/// \brief Compute a sorted list of faces of a polyhedron given a list of points -/// \param[in] start Start iterator of the list of points -/// \param[in] end End iterator of the list of points -/// \return The list of faces -template -std::vector get_sorted_face_list(const Iter start, const Iter end) -{ - // First get a sorted list of points - convex_hull does that by modifying its argument - auto corner_list = std::list(start, end); - const auto first_interior_point = convex_hull(corner_list); - - std::vector face_list{}; - auto itLast = corner_list.begin(); - auto itNext = std::next(itLast, 1); - do { - face_list.emplace_back(Line{*itLast, *itNext}); - itLast = itNext; - itNext = std::next(itNext, 1); - } while ((itNext != first_interior_point) && (itNext != corner_list.end())); - - face_list.emplace_back(Line{*itLast, corner_list.front()}); - - return face_list; -} - -/// \brief Append points of the polygon `internal` that are contained in the polygon `external`. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -void append_contained_points( - const Iterable1T & external, const Iterable2T & internal, - std::list & result) -{ - std::copy_if( - internal.begin(), internal.end(), std::back_inserter(result), [&external](const auto & pt) { - return common::geometry::is_point_inside_polygon_2d(external.begin(), external.end(), pt); - }); -} - -/// \brief Append the intersecting points between two polygons into the output list. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -void append_intersection_points( - const Iterable1T & polygon1, const Iterable2T & polygon2, - std::list & result) -{ - using FloatT = decltype(point_adapter::x_(std::declval())); - using Interval = common::geometry::Interval; - - auto get_edge = [](const auto & list, const auto & iterator) { - const auto next_it = std::next(iterator); - const auto & next_pt = (next_it != list.end()) ? *next_it : list.front(); - return std::make_pair(*iterator, next_pt); - }; - - // Get the max absolute value out of two intervals and a scalar. - auto compute_eps_scale = [](const auto & i1, const auto & i2, const auto val) { - auto get_abs_max = [](const auto & interval) { - return std::max(std::fabs(Interval::min(interval)), std::fabs(Interval::max(interval))); - }; - return std::max(std::fabs(val), std::max(get_abs_max(i1), get_abs_max(i2))); - }; - - // Compare each edge from polygon1 to each edge from polygon2 - for (auto corner1_it = polygon1.begin(); corner1_it != polygon1.end(); ++corner1_it) { - const auto & edge1 = get_edge(polygon1, corner1_it); - - Interval edge1_x_interval{ - std::min(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second)), - std::max(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second))}; - - Interval edge1_y_interval{ - std::min(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second)), - std::max(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second))}; - - for (auto corner2_it = polygon2.begin(); corner2_it != polygon2.end(); ++corner2_it) { - try { - const auto & edge2 = get_edge(polygon2, corner2_it); - const auto & intersection = common::geometry::intersection_2d( - edge1.first, minus_2d(edge1.second, edge1.first), edge2.first, - minus_2d(edge2.second, edge2.first)); - - Interval edge2_x_interval{ - std::min(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second)), - std::max(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second))}; - - Interval edge2_y_interval{ - std::min(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second)), - std::max(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second))}; - - // cspell: ignore feps - // feps means "Float EPSilon" - - // The accumulated floating point error depends on the magnitudes of each end of the - // intervals. Hence the upper bound of the absolute magnitude should be taken into account - // while computing the epsilon. - const auto max_feps_scale = std::max( - compute_eps_scale(edge1_x_interval, edge2_x_interval, point_adapter::x_(intersection)), - compute_eps_scale(edge1_y_interval, edge2_y_interval, point_adapter::y_(intersection))); - const auto feps = max_feps_scale * std::numeric_limits::epsilon(); - // Only accept intersections that lie on both of the line segments (edges) - if ( - Interval::contains(edge1_x_interval, point_adapter::x_(intersection), feps) && - Interval::contains(edge2_x_interval, point_adapter::x_(intersection), feps) && - Interval::contains(edge1_y_interval, point_adapter::y_(intersection), feps) && - Interval::contains(edge2_y_interval, point_adapter::y_(intersection), feps)) { - result.push_back(intersection); - } - } catch (const std::runtime_error &) { - // Parallel lines. TODO(yunus.caliskan): #1229 - continue; - } - } - } -} - -} // namespace details - -// TODO(s.me) implement GJK(+EPA) algorithm as well as per Chris Ho's suggestion -/// \tparam Iter Iterator over point-types that must have point adapters -// defined or have float members x and y -/// \brief Check if polyhedra defined by two given sets of points intersect -// This uses SAT for doing the actual checking -// (https://en.wikipedia.org/wiki/Hyperplane_separation_theorem#Use_in_collision_detection) -/// \param[in] begin1 Start iterator to first list of point types -/// \param[in] end1 End iterator to first list of point types -/// \param[in] begin2 Start iterator to first list of point types -/// \param[in] end2 End iterator to first list of point types -/// \return true if the boxes collide, false otherwise. -template -bool intersect(const Iter begin1, const Iter end1, const Iter begin2, const Iter end2) -{ - // Obtain sorted lists of faces of both boxes, merge them into one big list of faces - auto faces = details::get_sorted_face_list(begin1, end1); - const auto faces_2 = details::get_sorted_face_list(begin2, end2); - faces.reserve(faces.size() + faces_2.size()); - faces.insert(faces.end(), faces_2.begin(), faces_2.end()); - - // Also look at last line - for (const auto & face : faces) { - // Compute normal vector to the face and define a closure to get progress along it - const auto normal = get_normal(minus_2d(face.second, face.first)); - auto get_position_along_line = [&normal](auto point) { - return dot_2d(normal, minus_2d(point, Point{})); - }; - - // Define a function to get the minimum and maximum projected position of the corners - // of a given bounding box along the normal line of the face - auto get_projected_min_max = [&get_position_along_line, &normal](Iter begin, Iter end) { - const auto zero_point = Point{}; - auto min_corners = get_position_along_line(closest_line_point_2d(normal, zero_point, *begin)); - auto max_corners = min_corners; - - for (auto & point = begin; point != end; ++point) { - const auto point_projected = closest_line_point_2d(normal, zero_point, *point); - const auto position_along_line = get_position_along_line(point_projected); - min_corners = std::min(min_corners, position_along_line); - max_corners = std::max(max_corners, position_along_line); - } - return std::pair{min_corners, max_corners}; - }; - - // Perform the actual computations for the extent computation - auto minmax_1 = get_projected_min_max(begin1, end1); - auto minmax_2 = get_projected_min_max(begin2, end2); - - // Check for any intersections - const auto eps = std::numeric_limits::epsilon(); - if (minmax_1.first > minmax_2.second + eps || minmax_2.first > minmax_1.second + eps) { - // Found separating hyperplane, stop - return false; - } - } - - // No separating hyperplane found, boxes collide - return true; -} - -/// \brief Get the intersection between two polygons. The polygons should be provided in an -/// identical format to the output of `convex_hull` function as in the corners should be ordered -/// in a CCW fashion. -/// The computation is done by: -/// * Find the corners of each polygon that are contained by the other polygon. -/// * Find the intersection points between two polygons -/// * Combine these points and order CCW to get the final polygon. -/// The criteria for intersection is better explained in: -/// "Area of intersection of arbitrary polygons" (Livermore, Calif, 1977) -/// See https://www.osti.gov/servlets/purl/7309916/, chapter II - B -/// TODO(yunus.caliskan): This is a naive implementation. We should scan for a more efficient -/// algorithm: #1230 -/// \tparam Iterable1T A container class that has stl style iterators defined. -/// \tparam Iterable2T A container class that has stl style iterators defined. -/// \tparam PointT Point type that have the adapters for the x and y fields. -/// set to `Point1T` -/// \param polygon1 A convex polygon -/// \param polygon2 A convex polygon -/// \return The resulting conv -template < - template class Iterable1T, template class Iterable2T, typename PointT> -std::list convex_polygon_intersection2d( - const Iterable1T & polygon1, const Iterable2T & polygon2) -{ - std::list result; - details::append_contained_points(polygon1, polygon2, result); - details::append_contained_points(polygon2, polygon1, result); - details::append_intersection_points(polygon1, polygon2, result); - const auto end_it = common::geometry::convex_hull(result); - result.resize(static_cast(std::distance(result.cbegin(), end_it))); - return result; -} - -/// \brief Compute the intersection over union of two 2d convex polygons. If any of the polygons -/// span a zero area, the result is 0.0. -/// \tparam Iterable1T A container class that has stl style iterators defined. -/// \tparam Iterable2T A container class that has stl style iterators defined. -/// \tparam Point1T Point type that have the adapters for the x and y fields. -/// \tparam Point2T Point type that have the adapters for the x and y fields. -/// \param polygon1 A convex polygon -/// \param polygon2 A convex polygon -/// \return (Intersection / Union) between two given polygons. -/// \throws std::domain_error If there is any inconsistency on the underlying geometrical -/// computation. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -common::types::float32_t convex_intersection_over_union_2d( - const Iterable1T & polygon1, const Iterable2T & polygon2) -{ - constexpr auto eps = std::numeric_limits::epsilon(); - const auto intersection = convex_polygon_intersection2d(polygon1, polygon2); - - const auto intersection_area = - common::geometry::area_2d(intersection.begin(), intersection.end()); - - if (intersection_area < eps) { - return 0.0F; // There's either no intersection or the points are collinear - } - - const auto polygon1_area = common::geometry::area_2d(polygon1.begin(), polygon1.end()); - const auto polygon2_area = common::geometry::area_2d(polygon2.begin(), polygon2.end()); - - const auto union_area = polygon1_area + polygon2_area - intersection_area; - if (union_area < eps) { - throw std::domain_error("IoU is undefined for polygons with a zero union area"); - } - - return intersection_area / union_area; -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp deleted file mode 100644 index 54be2c32b1d05..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp +++ /dev/null @@ -1,358 +0,0 @@ -// Copyright 2020 Mapless AI, Inc. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/** - * @brief Data structure to contain scalar interval bounds. - * - * @post The interval is guaranteed to be valid upon successful construction. An - * interval [min, max] is "valid" if it is empty (min/max = NaN) or its bounds - * are ordered (min <= max). - */ -template -class Interval -{ - static_assert(std::is_floating_point::value, "Intervals only support floating point types."); - -public: - /** - * @brief Compute equality. - * - * Two intervals compare equal iff they are both valid and they are both - * either empty or have equal bounds. - * - * @note This is defined inline because the class is templated. - * - * @return True iff the intervals compare equal. - */ - friend bool operator==(const Interval & i1, const Interval & i2) - { - const auto min_eq = (Interval::min(i1) == Interval::min(i2)); - const auto max_eq = (Interval::max(i1) == Interval::max(i2)); - const auto bounds_equal = (min_eq && max_eq); - const auto both_empty = (Interval::empty(i1) && Interval::empty(i2)); - return both_empty || bounds_equal; - } - - /** - * @brief Compute inequality and the logical negation of equality. - * @note This is defined inline because the class is templated. - */ - friend bool operator!=(const Interval & i1, const Interval & i2) { return !(i1 == i2); } - - /** - * @brief Stream overload for Interval types. - * - * @note Output precision is fixed inside the function definition, and the - * serialization is JSON compatible. - * - * @note The serialization is lossy. It is used for debugging and for - * generating exception strings. - */ - friend std::ostream & operator<<(std::ostream & os, const Interval & i) - { - constexpr auto PRECISION = 5; - - // Internal helper to format the output. - const auto readable_extremum = [](const T & val) { - if (val == std::numeric_limits::lowest()) { - return std::string("REAL_MIN"); - } - if (val == std::numeric_limits::max()) { - return std::string("REAL_MAX"); - } - - std::stringstream ss; - ss.setf(std::ios::fixed, std::ios::floatfield); - ss.precision(PRECISION); - ss << val; - return ss.str(); - }; - - // Do stream output. - if (Interval::empty(i)) { - return os << "{}"; - } - return os << "{\"min\": " << readable_extremum(Interval::min(i)) - << ", \"max\": " << readable_extremum(Interval::max(i)) << "}"; - } - - /** - * @brief Test whether the two intervals have bounds within epsilon of each - * other. - * - * @note If both intervals are empty, this returns true. If only one is empty, - * this returns false. - */ - static bool abs_eq(const Interval & i1, const Interval & i2, const T & eps); - - /** @brief The minimum bound of the interval. */ - static T min(const Interval & i) { return i.min_; } - - /** @brief The maximum bound of the interval. */ - static T max(const Interval & i) { return i.max_; } - - /** - * @brief Return the measure (length) of the interval. - * - * @note For empty or invalid intervals, NaN is returned. See Interval::empty - * for note on distinction between measure zero and the emptiness property. - */ - static T measure(const Interval & i); - - /** - * @brief Utility for checking whether an interval has zero measure. - * - * @note For empty or invalid intervals, false is returned. See - * Interval::empty for note on distinction between measure zero and the - * emptiness property. - * - * @return True iff the interval has zero measure. - */ - static bool zero_measure(const Interval & i); - - /** - * @brief Whether the interval is empty or not. - * - * @note Emptiness refers to whether the interval contains any points and is - * thus a distinct property from measure: an interval is non-empty if contains - * only a single point even though its measure in that case is zero. - * - * @return True iff the interval is empty. - */ - static bool empty(const Interval & i); - - /** - * @brief Test for whether a given interval contains a given value within the given epsilon - * @return True iff 'interval' contains 'value'. - */ - static bool contains( - const Interval & i, const T & value, const T & eps = std::numeric_limits::epsilon()); - - /** - * @brief Test for whether 'i1' subseteq 'i2' - * @return True iff i1 subseteq i2. - */ - static bool is_subset_eq(const Interval & i1, const Interval & i2); - - /** - * @brief Compute the intersection of two intervals as a new interval. - */ - static Interval intersect(const Interval & i1, const Interval & i2); - - /** - * @brief Clamp a scalar 'val' onto 'interval'. - * @return If 'val' in 'interval', return 'val'; otherwise return the nearer - * interval bound. - */ - static T clamp_to(const Interval & i, T val); - - /** - * @brief Constructor: initialize an empty interval with members set to NaN. - */ - Interval(); - - /** - * @brief Constructor: specify exact interval bounds. - * - * @note An empty interval is specified by setting both bounds to NaN. - * @note An exception is thrown if the specified bounds are invalid. - * - * @post Construction is successful iff the interval is valid. - */ - Interval(const T & min, const T & max); - -private: - static constexpr T NaN = std::numeric_limits::quiet_NaN(); - - T min_; - T max_; - - /** - * @brief Verify that the bounds are valid in an interval. - * @note This method is private because it can only be used in the - * constructor. Once an interval has been constructed, its bounds are - * guaranteed to be valid. - */ - static bool bounds_valid(const Interval & i); -}; // class Interval - -//------------------------------------------------------------------------------ - -typedef Interval Interval_d; -typedef Interval Interval_f; - -//------------------------------------------------------------------------------ - -template -constexpr T Interval::NaN; - -//------------------------------------------------------------------------------ - -template -Interval::Interval() : min_(Interval::NaN), max_(Interval::NaN) -{ -} - -//------------------------------------------------------------------------------ - -template -Interval::Interval(const T & min, const T & max) : min_(min), max_(max) -{ - if (!Interval::bounds_valid(*this)) { - std::stringstream ss; - ss << "Attempted to construct an invalid interval: " << *this; - throw std::runtime_error(ss.str()); - } -} - -//------------------------------------------------------------------------------ - -template -bool Interval::abs_eq(const Interval & i1, const Interval & i2, const T & eps) -{ - namespace comp = helper_functions::comparisons; - - const auto both_empty = Interval::empty(i1) && Interval::empty(i2); - const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2); - - const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps); - const auto maxes_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps); - const auto both_non_empty_equal = both_non_empty && mins_equal && maxes_equal; - - return both_empty || both_non_empty_equal; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::zero_measure(const Interval & i) -{ - // An empty interval will return false because (NaN == NaN) is false. - return Interval::min(i) == Interval::max(i); -} - -//------------------------------------------------------------------------------ - -template -bool Interval::empty(const Interval & i) -{ - return std::isnan(Interval::min(i)) && std::isnan(Interval::max(i)); -} - -//------------------------------------------------------------------------------ - -template -bool Interval::bounds_valid(const Interval & i) -{ - const auto is_ordered = (Interval::min(i) <= Interval::max(i)); - - // Check for emptiness explicitly because it requires both bounds to be NaN - return Interval::empty(i) || is_ordered; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::is_subset_eq(const Interval & i1, const Interval & i2) -{ - const auto lower_contained = (Interval::min(i1) >= Interval::min(i2)); - const auto upper_contained = (Interval::max(i1) <= Interval::max(i2)); - return lower_contained && upper_contained; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::contains(const Interval & i, const T & value, const T & eps) -{ - return common::helper_functions::comparisons::abs_gte(value, Interval::min(i), eps) && - common::helper_functions::comparisons::abs_lte(value, Interval::max(i), eps); -} - -//------------------------------------------------------------------------------ - -template -T Interval::measure(const Interval & i) -{ - return Interval::max(i) - Interval::min(i); -} - -//------------------------------------------------------------------------------ - -template -Interval Interval::intersect(const Interval & i1, const Interval & i2) -{ - // Construct intersection assuming an intersection exists. - try { - const auto either_empty = Interval::empty(i1) || Interval::empty(i2); - const auto min = std::max(Interval::min(i1), Interval::min(i2)); - const auto max = std::min(Interval::max(i1), Interval::max(i2)); - return either_empty ? Interval() : Interval(min, max); - } catch (...) { - } - - // Otherwise, the intersection is empty. - return Interval(); -} - -//------------------------------------------------------------------------------ - -template -T Interval::clamp_to(const Interval & i, T val) -{ - // clamp val to min - val = std::max(val, Interval::min(i)); - - // clamp val to max - val = std::min(val, Interval::max(i)); - - return Interval::empty(i) ? Interval::NaN : val; -} - -//------------------------------------------------------------------------------ - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp deleted file mode 100644 index 7b8867ca096ae..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file contains a 1D linear lookup table implementation - -#ifndef AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace helper_functions -{ - -namespace -{ - -/** - * @brief Compute a scaling between 'a' and 'b' - * @note scaling is clamped to be in the interval [0, 1] - */ -template -T interpolate(const T & a, const T & b, U scaling) -{ - static const geometry::Interval UNIT_INTERVAL(static_cast(0), static_cast(1)); - scaling = geometry::Interval::clamp_to(UNIT_INTERVAL, scaling); - - const auto m = (b - a); - const auto offset = static_cast(m) * scaling; - return a + static_cast(offset); -} - -// TODO(c.ho) support more forms of interpolation as template functor -// Actual lookup logic, assuming all invariants hold: -// Throw if value is not finite -template -T lookup_impl_1d(const std::vector & domain, const std::vector & range, const T value) -{ - if (!std::isfinite(value)) { - throw std::domain_error{"Query value is not finite (NAN or INF)"}; - } - if (value <= domain.front()) { - return range.front(); - } else if (value >= domain.back()) { - return range.back(); - } else { - // Fall through to normal case - } - - auto second_idx{0U}; - for (auto idx = 1U; idx < domain.size(); ++idx) { - if (value < domain[idx]) { - second_idx = idx; - break; - } - } - // T must be a floating point between 0 and 1 - const auto num = static_cast(value - domain[second_idx - 1U]); - const auto den = static_cast(domain[second_idx] - domain[second_idx - 1U]); - const auto t = num / den; - const auto val = interpolate(range[second_idx - 1U], range[second_idx], t); - return static_cast(val); -} - -// Check invariants for table lookup: -template -void check_table_lookup_invariants(const std::vector & domain, const std::vector & range) -{ - if (domain.size() != range.size()) { - throw std::domain_error{"Domain's size does not match range's"}; - } - if (domain.empty()) { - throw std::domain_error{"Empty domain or range"}; - } - // Check if sorted: Can start at 1 because not empty - for (auto idx = 1U; idx < domain.size(); ++idx) { - if (domain[idx] <= domain[idx - 1U]) { // This is safe because indexing starts at 1 - throw std::domain_error{"Domain is not sorted"}; - } - } -} -} // namespace - -/// Do a 1D table lookup: Does some semi-expensive O(N) error checking first. -/// If query value fall out of the domain, then the value at the corresponding edge of the domain is -/// returned. -/// \param[in] domain The domain, or set of x values -/// \param[in] range The range, or set of y values -/// \param[in] value The point in the domain to query, x -/// \return A linearly interpolated value y, corresponding to the query, x -/// \throw std::domain_error If domain or range is empty -/// \throw std::domain_error If range is not the same size as domain -/// \throw std::domain_error If domain is not sorted -/// \throw std::domain_error If value is not finite (NAN or INF) -/// \tparam T The type of the function, must be interpolatable -template -T lookup_1d(const std::vector & domain, const std::vector & range, const T value) -{ - check_table_lookup_invariants(domain, range); - - return lookup_impl_1d(domain, range, value); -} - -/// A class wrapping a 1D lookup table. Intended for more frequent lookups. Error checking is pushed -/// into the constructor and not done in the lookup function call -/// \tparam T The type of the function, must be interpolatable -template -class LookupTable1D -{ -public: - /// Constructor - /// \param[in] domain The domain, or set of x values - /// \param[in] range The range, or set of y values - /// \throw std::domain_error If domain or range is empty - /// \throw std::domain_error If range is not the same size as domain - /// \throw std::domain_error If domain is not sorted - LookupTable1D(const std::vector & domain, const std::vector & range) - : m_domain{domain}, m_range{range} - { - check_table_lookup_invariants(m_domain, m_range); - } - - /// Move constructor - /// \param[in] domain The domain, or set of x values - /// \param[in] range The range, or set of y values - /// \throw std::domain_error If domain or range is empty - /// \throw std::domain_error If range is not the same size as domain - /// \throw std::domain_error If domain is not sorted - LookupTable1D(std::vector && domain, std::vector && range) - : m_domain{domain}, m_range{range} - { - check_table_lookup_invariants(m_domain, m_range); - } - - /// Do a 1D table lookup - /// If query value fall out of the domain, then the value at the corresponding edge of the domain - /// is returned. - /// \param[in] value The point in the domain to query, x - /// \return A linearly interpolated value y, corresponding to the query, x - /// \throw std::domain_error If value is not finite - T lookup(const T value) const { return lookup_impl_1d(m_domain, m_range, value); } - /// Get the domain table - const std::vector & domain() const noexcept { return m_domain; } - /// Get the range table - const std::vector & range() const noexcept { return m_range; } - -private: - std::vector m_domain; - std::vector m_range; -}; // class LookupTable1D - -} // namespace helper_functions -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp deleted file mode 100644 index 8936e2c17d779..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp +++ /dev/null @@ -1,332 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in -/// 2D - -#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ - -#include "autoware_auto_geometry/spatial_hash_config.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include - -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup -namespace spatial_hash -{ - -/// \brief An implementation of the spatial hash or integer lattice data structure for efficient -/// (O(1)) near neighbor queries. -/// \tparam PointT The point type stored in this data structure. Must have float members x, y, and z -/// -/// This implementation can support both 2D and 3D queries -/// (though only one type per data structure), and can support queries of varying radius. This data -/// structure cannot do near neighbor lookups for euclidean distance in arbitrary dimensions. -template -class GEOMETRY_PUBLIC SpatialHashBase -{ - using Index3 = details::Index3; - // lint -e{9131} NOLINT There's no other way to make this work in a static assert - static_assert( - std::is_same::value || std::is_same::value, - "SpatialHash only works with Config2d or Config3d"); - -public: - using Hash = std::unordered_multimap; - using IT = typename Hash::const_iterator; - /// \brief Wrapper around an iterator and a distance (from some query point) - class Output - { - public: - /// \brief Constructor - /// \param[in] iterator An iterator pointing to some point - /// \param[in] distance The euclidean distance (2d or 3d) to a reference point - Output(const IT iterator, const float32_t distance) : m_iterator(iterator), m_distance(distance) - { - } - /// \brief Get stored point - /// \return A const reference to the stored point - const PointT & get_point() const { return m_iterator->second; } - /// \brief Get underlying iterator - /// \return A copy of the underlying iterator - IT get_iterator() const { return m_iterator; } - /// \brief Convert to underlying point - /// \return A reference to the underlying point - operator const PointT &() const { return get_point(); } - /// \brief Convert to underlying iterator - /// \return A copy of the iterator - operator IT() const { return get_iterator(); } - /// \brief Get distance to reference point - /// \return The distance - float32_t get_distance() const { return m_distance; } - - private: - IT m_iterator; - float32_t m_distance; - }; // class Output - using OutputVector = typename std::vector; - - /// \brief Constructor - /// \param[in] cfg The configuration object for this class - explicit SpatialHashBase(const ConfigT & cfg) - : m_config{cfg}, - m_hash(), - m_neighbors{}, // TODO(c.ho) reserve, but there's no default constructor for output - m_bins_hit{}, // zero initialization (and below) - m_neighbors_found{} - { - } - - /// \brief Inserts point - /// \param[in] pt The Point to insert - /// \return Iterator pointing to the inserted point - /// \throw std::length_error If the data structure is at capacity - IT insert(const PointT & pt) - { - if (size() >= capacity()) { - throw std::length_error{"SpatialHash: Cannot insert past capacity"}; - } - return insert_impl(pt); - } - - /// \brief Inserts a range of points - /// \param[in] begin The start of the range of points to insert - /// \param[in] end The end of the range of points to insert - /// \tparam IteratorT The iterator type - /// \throw std::length_error If the range of points to insert exceeds the data structure's - /// capacity - template - void insert(IteratorT begin, IteratorT end) - { - // This check is here for strong exception safety - if ((size() + std::distance(begin, end)) > capacity()) { - throw std::length_error{"SpatialHash: Cannot multi-insert past capacity"}; - } - for (IteratorT it = begin; it != end; ++it) { - (void)insert_impl(*it); - } - } - - /// \brief Removes the specified element from the data structure - /// \param[in] point An iterator pointing to a point to be removed - /// \return An iterator pointing to the element after the erased element - /// \throw std::domain_error If pt is invalid or does not belong to this data structure - /// - /// \note There is no reliable way to check if an iterator is invalid. The checks here are - /// based on a heuristic and is not guaranteed to find all invalid iterators. This method - /// should be used with care and only on valid iterators - IT erase(const IT point) - { - if (end() == m_hash.find(point->first)) { - throw std::domain_error{"SpatialHash: Attempting to erase invalid iterator"}; - } - return m_hash.erase(point); - } - - /// \brief Reset the state of the data structure - void clear() { m_hash.clear(); } - /// \brief Get current number of element stored in this data structure - /// \return Number of stored elements - Index size() const { return m_hash.size(); } - /// \brief Get the maximum capacity of the data structure - /// \return The capacity of the data structure - Index capacity() const { return m_config.get_capacity(); } - /// \brief Whether the hash is empty - /// \return True if data structure is empty - bool8_t empty() const { return m_hash.empty(); } - /// \brief Get iterator to beginning of data structure - /// \return Iterator - IT begin() const { return m_hash.begin(); } - /// \brief Get iterator to end of data structure - /// \return Iterator - IT end() const { return m_hash.end(); } - /// \brief Get iterator to beginning of data structure - /// \return Iterator - IT cbegin() const { return begin(); } - /// \brief Get iterator to end of data structure - /// \return Iterator - IT cend() const { return end(); } - - /// \brief Get the number of bins touched during the lifetime of this object, for debugging and - /// size tuning - /// \return The total number of bins touched during near() queries - Index bins_hit() const { return m_bins_hit; } - - /// \brief Get number of near neighbors found during the lifetime of this object, for debugging - /// and size tuning - /// \return The total number of neighbors found during near() queries - Index neighbors_found() const { return m_neighbors_found; } - -protected: - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] z The z component of the reference point, respected only if the spatial hash is not - /// 2D. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near_impl( - const float32_t x, const float32_t y, const float32_t z, const float32_t radius) - { - // reset output - m_neighbors.clear(); - // Compute bin, bin range - const Index3 ref_idx = m_config.index3(x, y, z); - const float32_t radius2 = radius * radius; - const details::BinRange idx_range = m_config.bin_range(ref_idx, radius); - Index3 idx = idx_range.first; - // For bins in radius - do { // guaranteed to have at least the bin ref_idx is in - // update book-keeping - ++m_bins_hit; - // Iterating in a square/cube pattern is easier than constructing sphere pattern - if (m_config.is_candidate_bin(ref_idx, idx, radius2)) { - // For point in bin - const Index jdx = m_config.index(idx); - const auto range = m_hash.equal_range(jdx); - for (auto it = range.first; it != range.second; ++it) { - const auto & pt = it->second; - const float32_t dist2 = m_config.distance_squared(x, y, z, pt); - if (dist2 <= radius2) { - // Only compute true distance if necessary - m_neighbors.emplace_back(it, sqrtf(dist2)); - } - } - } - } while (m_config.next_bin(idx_range, idx)); - // update book-keeping - m_neighbors_found += m_neighbors.size(); - return m_neighbors; - } - -private: - /// \brief Internal insert method with no error checking - /// \param[in] pt The Point to insert - GEOMETRY_LOCAL IT insert_impl(const PointT & pt) - { - // Compute bin - const Index idx = - m_config.bin(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt)); - // Insert into bin - return m_hash.insert(std::make_pair(idx, pt)); - } - - const ConfigT m_config; - Hash m_hash; - OutputVector m_neighbors; - Index m_bins_hit; - Index m_neighbors_found; -}; // class SpatialHashBase - -/// \brief The class to be used for specializing on -/// apex_app::common::geometry::spatial_hash::SpatialHashBase to provide different function -/// signatures on 2D and 3D configurations -/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z -template -class GEOMETRY_PUBLIC SpatialHash; - -/// \brief Explicit specialization of SpatialHash for 2D configuration -/// \tparam PointT The point type stored in this data structure. -template -class GEOMETRY_PUBLIC SpatialHash : public SpatialHashBase -{ -public: - using OutputVector = typename SpatialHashBase::OutputVector; - - explicit SpatialHash(const Config2d & cfg) : SpatialHashBase(cfg) {} - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const float32_t x, const float32_t y, const float32_t radius) - { - return this->near_impl(x, y, 0.0F, radius); - } - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] pt The reference point. Only the x and y members are respected. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const PointT & pt, const float32_t radius) - { - return near(point_adapter::x_(pt), point_adapter::y_(pt), radius); - } -}; - -/// \brief Explicit specialization of SpatialHash for 3D configuration -/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z -template -class GEOMETRY_PUBLIC SpatialHash : public SpatialHashBase -{ -public: - using OutputVector = typename SpatialHashBase::OutputVector; - - explicit SpatialHash(const Config3d & cfg) : SpatialHashBase(cfg) {} - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] z The z component of the reference point, respected only if the spatial hash is not - /// 2D. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near( - const float32_t x, const float32_t y, const float32_t z, const float32_t radius) - { - return this->near_impl(x, y, z, radius); - } - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] pt The reference point. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const PointT & pt, const float32_t radius) - { - return near(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt), radius); - } -}; - -template -using SpatialHash2d = SpatialHash; -template -using SpatialHash3d = SpatialHash; -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp deleted file mode 100644 index 24c4d6e879d4a..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp +++ /dev/null @@ -1,450 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in -/// 2D - -#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup -namespace spatial_hash -{ -/// \brief Index type for identifying bins of the hash/lattice -using Index = std::size_t; -/// \brief Spatial hash functionality not intended to be used by an external user -namespace details -{ -/// \brief Internal struct for packing three indices together -/// -/// The use of this struct publicly is a violation of our coding standards, but I claim it's -/// fine because (a) it's details, (b) it is literally three unrelated members packaged together. -/// This type is needed for conceptual convenience so I don't have massive function parameter -/// lists -struct GEOMETRY_PUBLIC Index3 -{ - Index x; - Index y; - Index z; -}; // struct Index3 - -using BinRange = std::pair; -} // namespace details - -/// \brief The base class for the configuration object for the SpatialHash class -/// \tparam Derived The type of the derived class to support static polymorphism/CRTP -template -class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp -{ -public: - /// \brief Constructor for spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] min_z The minimum z value for the spatial hash - /// \param[in] max_z The maximum z value for the spatial hash - /// \param[in] radius The look up radius - /// \param[in] capacity The maximum number of points the spatial hash can store - Config( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity) - : m_min_x{min_x}, - m_min_y{min_y}, - m_min_z{min_z}, - m_max_x{max_x}, - m_max_y{max_y}, - m_max_z{max_z}, - m_side_length{radius}, - m_side_length2{radius * radius}, - m_side_length_inv{1.0F / radius}, - m_capacity{capacity}, - m_max_x_idx{check_basis_direction(min_x, max_x)}, - m_max_y_idx{check_basis_direction(min_y, max_y)}, - m_max_z_idx{check_basis_direction(min_z, max_z)}, - m_y_stride{m_max_x_idx + 1U}, - m_z_stride{m_y_stride * (m_max_y_idx + 1U)} - { - if (radius <= 0.0F) { - throw std::domain_error("Error constructing SpatialHash: must have positive side length"); - } - - if ((m_max_y_idx + m_y_stride) > std::numeric_limits::max() / 2U) { - // TODO(c.ho) properly check for multiplication overflow - throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); - } - // small fudging to prevent weird boundary effects - // (e.g (x=x_max, y) rolls index over to (x=0, y+1) - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - // lint -e{1938} read only access is fine NOLINT - m_max_x -= FEPS; - m_max_y -= FEPS; - m_max_z -= FEPS; - if ((m_z_stride + m_max_z_idx) > std::numeric_limits::max() / 2U) { - // TODO(c.ho) properly check for multiplication overflow - throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); - } - // I don't know if this is even possible given other checks - if (std::numeric_limits::max() == m_max_z_idx) { - throw std::domain_error("SpatialHash::Config: max z index exceeds reasonable value"); - } - } - - /// \brief Given a reference index triple, compute the first and last bin - /// \param[in] ref The reference index triple - /// \param[in] radius The allowable radius for any point in the reference bin to any point in the - /// range - /// \return A pair where the first element is an index triple of the first bin, and the second - /// element is an index triple for the last bin - details::BinRange bin_range(const details::Index3 & ref, const float radius) const - { - // Compute distance in units of voxels - const Index i_radius = static_cast(std::ceil(radius / m_side_length)); - // Dumb ternary because potentially unsigned Index type - const Index x_min = (ref.x > i_radius) ? (ref.x - i_radius) : 0U; - const Index y_min = (ref.y > i_radius) ? (ref.y - i_radius) : 0U; - const Index z_min = (ref.z > i_radius) ? (ref.z - i_radius) : 0U; - // In 2D mode, m_max_z should be 0, same with ref.z - const Index x_max = std::min(ref.x + i_radius, m_max_x_idx); - const Index y_max = std::min(ref.y + i_radius, m_max_y_idx); - const Index z_max = std::min(ref.z + i_radius, m_max_z_idx); - // return bottom-left portion of cube and top-right portion of cube - return {{x_min, y_min, z_min}, {x_max, y_max, z_max}}; - } - - /// \brief Get next index within a given range - /// \return True if idx is valid and still in range - /// \param[in] range The max and min bin indices - /// \param[inout] idx The index to be incremented, updated even if a negative result occurs - bool8_t next_bin(const details::BinRange & range, details::Index3 & idx) const - { - // TODO(c.ho) is there any way to make this neater without triple nested if? - bool8_t ret = true; - ++idx.x; - if (idx.x > range.second.x) { - idx.x = range.first.x; - ++idx.y; - if (idx.y > range.second.y) { - idx.y = range.first.y; - ++idx.z; - if (idx.z > range.second.z) { - ret = false; - } - } - } - return ret; - } - /// \brief Get the maximum capacity of the spatial hash - /// \return The capacity - Index get_capacity() const { return m_capacity; } - - /// \brief Getter for the side length, equivalently the lookup radius - float32_t radius2() const { return m_side_length2; } - - //////////////////////////////////////////////////////////////////////////////////////////// - // "Polymorphic" API - /// \brief Compute the single index given a point - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The combined index of the bin for a given point - Index bin(const float32_t x, const float32_t y, const float32_t z) const - { - return this->impl().bin_(x, y, z); - } - /// \brief Compute whether the query bin and reference bin could possibly contain a pair of points - /// such that their distance is within a certain threshold - /// \param[in] ref The index triple of the bin containing the reference point - /// \param[in] query The index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if query and ref could possibly hold points within reference distance to one - /// another - bool is_candidate_bin( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const - { - return this->impl().valid(ref, query, ref_distance2); - } - /// \brief Compute the decomposed index given a point - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const - { - return this->impl().index3_(x, y, z); - } - /// \brief Compute the composed single index given a decomposed index - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index(const details::Index3 & idx) const { return this->impl().index_(idx); } - /// \brief Compute the squared distance between the two points - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (2d or 3d) - template - float32_t distance_squared( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - return this->impl().distance_squared_(x, y, z, pt); - } - -protected: - /// \brief Computes the index in the x basis direction - /// \param[in] x The x value of a point - /// \return The x offset of the index - Index x_index(const float32_t x) const - { - return static_cast( - std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv)); - } - /// \brief Computes the index in the y basis direction - /// \param[in] y The y value of a point - /// \return The x offset of the index - Index y_index(const float32_t y) const - { - return static_cast( - std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv)); - } - /// \brief Computes the index in the z basis direction - /// \param[in] z The z value of a point - /// \return The x offset of the index - Index z_index(const float32_t z) const - { - return static_cast( - std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv)); - } - /// \brief Compose the provided index offsets - Index bin_impl(const Index xdx, const Index ydx) const { return xdx + (ydx * m_y_stride); } - /// \brief Compose the provided index offsets - Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const - { - return bin_impl(xdx, ydx) + (zdx * m_z_stride); - } - - /// \brief The index offset of a point given it's x and y values - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \return The index of the point in the 2D case, or the offset within a z-slice of the hash - Index bin_impl(const float32_t x, const float32_t y) const - { - return bin_impl(x_index(x), y_index(y)); - } - /// \brief The index of a point given it's x, y and z values - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const - { - return bin_impl(x, y) + (z_index(z) * m_z_stride); - } - /// \brief The distance between two indices as a float, where adjacent indices have zero - /// distance (e.g. dist(0, 1) = 0) - float32_t idx_distance(const Index ref_idx, const Index query_idx) const - { - /// Not using fabs because Index is (possibly) unsigned - const Index i_dist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx); - float32_t dist = static_cast(i_dist) - 1.0F; - return std::max(dist, 0.0F); - } - - /// \brief Get side length squared - float side_length2() const { return m_side_length2; } - -private: - /// \brief Sanity check a range in a basis direction - /// \return The number of voxels in the given basis direction - Index check_basis_direction(const float32_t min, const float32_t max) const - { - if (min >= max) { - throw std::domain_error("SpatialHash::Config: must have min < max"); - } - // This family of checks is to ensure that you don't get weird casting effects due to huge - // floating point values - const float64_t dmax = static_cast(max); - const float64_t dmin = static_cast(min); - const float64_t width = (dmax - dmin) * static_cast(m_side_length_inv); - constexpr float64_t flt_max = static_cast(std::numeric_limits::max()); - if (flt_max <= width) { - throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit"); - } - return static_cast(width); - } - float32_t m_min_x; - float32_t m_min_y; - float32_t m_min_z; - float32_t m_max_x; - float32_t m_max_y; - float32_t m_max_z; - float32_t m_side_length; - float32_t m_side_length2; - float32_t m_side_length_inv; - Index m_capacity; - Index m_max_x_idx; - Index m_max_y_idx; - Index m_max_z_idx; - Index m_y_stride; - Index m_z_stride; -}; // class Config - -/// \brief Configuration class for a 2d spatial hash -class GEOMETRY_PUBLIC Config2d : public Config -{ -public: - /// \brief Config constructor for 2D spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] radius The lookup distance - /// \param[in] capacity The maximum number of points the spatial hash can store - Config2d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t radius, const Index capacity); - /// \brief The index of a point given it's x, y and z values, 2d implementation - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Determine if a bin could possibly hold a point within a distance to any point in a - /// reference bin for the 2D case - /// \param[in] ref The decomposed index triple of the reference bin - /// \param[in] query The decomposed index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if the reference bin and query bin could possibly hold a point within the - /// reference distance - bool valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const; - /// \brief Compute the decomposed index given a point, 2d implementation - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Compute the composed single index given a decomposed index, 2d implementation - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index_(const details::Index3 & idx) const; - /// \brief Compute the squared distance between the two points, 2d implementation - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (2d) - template - float32_t distance_squared_( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - (void)z; - const float32_t dx = x - point_adapter::x_(pt); - const float32_t dy = y - point_adapter::y_(pt); - return (dx * dx) + (dy * dy); - } -}; // class Config2d - -/// \brief Configuration class for a 3d spatial hash -class GEOMETRY_PUBLIC Config3d : public Config -{ -public: - /// \brief Config constructor for a 3d spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] min_z The minimum z value for the spatial hash - /// \param[in] max_z The maximum z value for the spatial hash - /// \param[in] radius The lookup distance - /// \param[in] capacity The maximum number of points the spatial hash can store - Config3d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity); - /// \brief The index of a point given it's x, y and z values, 3d implementation - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Determine if a bin could possibly hold a point within a distance to any point in a - /// reference bin for the 3D case - /// \param[in] ref The decomposed index triple of the reference bin - /// \param[in] query The decomposed index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if the reference bin and query bin could possibly hold a point within the - /// reference distance - bool valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const; - /// \brief Compute the decomposed index given a point, 3d implementation - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Compute the composed single index given a decomposed index, 3d implementation - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index_(const details::Index3 & idx) const; - /// \brief Compute the squared distance between the two points, 3d implementation - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (3d) - template - float32_t distance_squared_( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - const float32_t dx = x - point_adapter::x_(pt); - const float32_t dy = y - point_adapter::y_(pt); - const float32_t dz = z - point_adapter::z_(pt); - return (dx * dx) + (dy * dy) + (dz * dz); - } -}; // class Config3d -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp deleted file mode 100644 index 8972246997997..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllexport) -#define GEOMETRY_LOCAL -#else // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllimport) -#define GEOMETRY_LOCAL -#endif // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#elif defined(__linux__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(QNX) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#else // defined(__linux__) -#error "Unsupported Build Configuration" -#endif // defined(__WIN32) -#endif // AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_geometry/package.xml b/common/autoware_auto_geometry/package.xml deleted file mode 100644 index f53412298a485..0000000000000 --- a/common/autoware_auto_geometry/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_auto_geometry - 1.0.0 - Geometry related algorithms - Satoshi Ota - Apache License 2.0 - - Apex.AI, Inc. - - ament_cmake_auto - autoware_cmake - - autoware_auto_common - autoware_auto_geometry_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 - autoware_auto_vehicle_msgs - geometry_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - osrf_testing_tools_cpp - - - ament_cmake - - diff --git a/common/autoware_auto_geometry/src/bounding_box.cpp b/common/autoware_auto_geometry/src/bounding_box.cpp deleted file mode 100644 index 3a4ea96a151a2..0000000000000 --- a/common/autoware_auto_geometry/src/bounding_box.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" - -#include -// cspell: ignore eigenbox -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -// cspell: ignore lfit -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -#include - -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ -//////////////////////////////////////////////////////////////////////////////// -void size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret) -{ - ret.x = std::max( - norm_2d(minus_2d(corners[1U], corners[0U])), std::numeric_limits::epsilon()); - ret.y = std::max( - norm_2d(minus_2d(corners[2U], corners[1U])), std::numeric_limits::epsilon()); -} -//////////////////////////////////////////////////////////////////////////////// -void finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box) -{ - (void)std::copy(&corners[0U], &corners[corners.size()], &box.corners[0U]); - // orientation: this is robust to lines - const float32_t yaw_rad_2 = - atan2f(box.corners[2U].y - box.corners[1U].y, box.corners[2U].x - box.corners[1U].x) * 0.5F; - box.orientation.w = cosf(yaw_rad_2); - box.orientation.z = sinf(yaw_rad_2); - // centroid - box.centroid = times_2d(plus_2d(corners[0U], corners[2U]), 0.5F); -} - -autoware_auto_perception_msgs::msg::Shape make_shape(const BoundingBox & box) -{ - autoware_auto_perception_msgs::msg::Shape retval; - // Polygon is 2D rectangle - geometry_msgs::msg::Polygon polygon; - auto & points = polygon.points; - points.resize(4); - - // Note that the x and y of size field in BoundingBox should be swapped when being used to - // compute corners. - // origin of reference system: centroid of bbox - points[0].x = -0.5F * box.size.y; - points[0].y = -0.5F * box.size.x; - points[0].z = -box.size.z; - - points[1] = points[0]; - points[1].y += box.size.x; - - points[2] = points[1]; - points[2].x += box.size.y; - - points[3] = points[2]; - points[3].y -= box.size.x; - - retval.footprint = polygon; - retval.dimensions.z = 2 * box.size.z; - - return retval; -} - -autoware_auto_perception_msgs::msg::DetectedObject make_detected_object(const BoundingBox & box) -{ - autoware_auto_perception_msgs::msg::DetectedObject ret; - - ret.kinematics.pose_with_covariance.pose.position.x = static_cast(box.centroid.x); - ret.kinematics.pose_with_covariance.pose.position.y = static_cast(box.centroid.y); - ret.kinematics.pose_with_covariance.pose.position.z = static_cast(box.centroid.z); - ret.kinematics.pose_with_covariance.pose.orientation.x = static_cast(box.orientation.x); - ret.kinematics.pose_with_covariance.pose.orientation.y = static_cast(box.orientation.y); - ret.kinematics.pose_with_covariance.pose.orientation.z = static_cast(box.orientation.z); - ret.kinematics.pose_with_covariance.pose.orientation.w = static_cast(box.orientation.w); - ret.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - - ret.shape = make_shape(box); - - ret.existence_probability = 1.0F; - - autoware_auto_perception_msgs::msg::ObjectClassification label; - label.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; - label.probability = 1.0F; - ret.classification.emplace_back(std::move(label)); - - return ret; -} - -std::vector GEOMETRY_PUBLIC get_transformed_corners( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation) -{ - std::vector retval(shape_msg.footprint.points.size()); - geometry_msgs::msg::TransformStamped tf; - tf.transform.rotation = orientation; - tf.transform.translation.x = centroid.x; - tf.transform.translation.y = centroid.y; - tf.transform.translation.z = centroid.z; - - for (size_t i = 0U; i < shape_msg.footprint.points.size(); ++i) { - tf2::doTransform(shape_msg.footprint.points[i], retval[i], tf); - } - return retval; -} - -} // namespace details -/////////////////////////////////////////////////////////////////////////////// -// pre-compilation -using autoware::common::types::PointXYZIF; -template BoundingBox minimum_area_bounding_box(std::list & list); -template BoundingBox minimum_perimeter_bounding_box(std::list & list); -using PointXYZIFVIT = std::vector::iterator; -template BoundingBox eigenbox_2d(const PointXYZIFVIT begin, const PointXYZIFVIT end); -template BoundingBox lfit_bounding_box_2d( - const PointXYZIFVIT begin, const PointXYZIFVIT end); -using geometry_msgs::msg::Point32; -template BoundingBox minimum_area_bounding_box(std::list & list); -template BoundingBox minimum_perimeter_bounding_box(std::list & list); -using Point32VIT = std::vector::iterator; -template BoundingBox eigenbox_2d(const Point32VIT begin, const Point32VIT end); -template BoundingBox lfit_bounding_box_2d(const Point32VIT begin, const Point32VIT end); -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp deleted file mode 100644 index ffd91aaa08b3a..0000000000000 --- a/common/autoware_auto_geometry/src/spatial_hash.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/spatial_hash.hpp" - -#include -// lint -e537 NOLINT repeated include file due to cpplint rule -#include -// lint -e537 NOLINT repeated include file due to cpplint rule -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace spatial_hash -{ -//////////////////////////////////////////////////////////////////////////////// -Config2d::Config2d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t radius, const Index capacity) -: Config(min_x, max_x, min_y, max_y, {}, std::numeric_limits::min(), radius, capacity) -{ -} -//////////////////////////////////////////////////////////////////////////////// -Index Config2d::bin_(const float32_t x, const float32_t y, const float32_t z) const -{ - (void)z; - return bin_impl(x, y); -} -//////////////////////////////////////////////////////////////////////////////// -bool Config2d::valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const -{ - const float dx = idx_distance(ref.x, query.x); - const float dy = idx_distance(ref.y, query.y); - // minor algebraic manipulation happening below - return (((dx * dx) + (dy * dy)) * side_length2()) <= ref_distance2; -} -//////////////////////////////////////////////////////////////////////////////// -details::Index3 Config2d::index3_(const float32_t x, const float32_t y, const float32_t z) const -{ - (void)z; - return {x_index(x), y_index(y), Index{}}; // zero initialization -} -//////////////////////////////////////////////////////////////////////////////// -Index Config2d::index_(const details::Index3 & idx) const -{ - return bin_impl(idx.x, idx.y); -} -//////////////////////////////////////////////////////////////////////////////// -Config3d::Config3d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity) -: Config(min_x, max_x, min_y, max_y, min_z, max_z, radius, capacity) -{ -} -//////////////////////////////////////////////////////////////////////////////// -Index Config3d::bin_(const float32_t x, const float32_t y, const float32_t z) const -{ - return bin_impl(x, y, z); -} -//////////////////////////////////////////////////////////////////////////////// -bool Config3d::valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const -{ - const float dx = idx_distance(ref.x, query.x); - const float dy = idx_distance(ref.y, query.y); - const float dz = idx_distance(ref.z, query.z); - // minor algebraic manipulation happening below - return (((dx * dx) + (dy * dy) + (dz * dz)) * side_length2()) <= ref_distance2; -} -//////////////////////////////////////////////////////////////////////////////// -details::Index3 Config3d::index3_(const float32_t x, const float32_t y, const float32_t z) const -{ - return {x_index(x), y_index(y), z_index(z)}; // zero initialization -} -//////////////////////////////////////////////////////////////////////////////// -Index Config3d::index_(const details::Index3 & idx) const -{ - return bin_impl(idx.x, idx.y, idx.z); -} -//////////////////////////////////////////////////////////////////////////////// -template class SpatialHash; -template class SpatialHash; -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp deleted file mode 100644 index a179fbde5f151..0000000000000 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ /dev/null @@ -1,550 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef TEST_BOUNDING_BOX_HPP_ -#define TEST_BOUNDING_BOX_HPP_ - -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -// cspell: ignore lfit -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -#include - -#include -#include -#include - -using autoware::common::geometry::point_adapter::x_; -using autoware::common::geometry::point_adapter::xr_; -using autoware::common::geometry::point_adapter::y_; -using autoware::common::geometry::point_adapter::yr_; -using autoware_auto_perception_msgs::msg::BoundingBox; - -template -class BoxTest : public ::testing::Test -{ -protected: - std::list points; - BoundingBox box; - - void minimum_area_bounding_box() - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::minimum_area_bounding_box(points); - // apex_test_tools::memory_test::stop(); - } - - void minimum_perimeter_bounding_box() - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::minimum_perimeter_bounding_box(points); - // apex_test_tools::memory_test::stop(); - } - - // cspell: ignore eigenbox - template - void eigenbox(const IT begin, const IT end) - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::eigenbox_2d(begin, end); - // apex_test_tools::memory_test::stop(); - } - template - void lfit_bounding_box_2d(const IT begin, const IT end) - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::lfit_bounding_box_2d(begin, end); - // apex_test_tools::memory_test::stop(); - } - - PointT make(const float x, const float y) - { - PointT ret; - xr_(ret) = x; - yr_(ret) = y; - return ret; - } - - void check(const float cx, const float cy, const float sx, const float sy, const float val) const - { - constexpr float32_t TOL = 1.0E-4F; - ASSERT_LT(fabsf(box.size.x - sx), TOL); - ASSERT_LT(fabsf(box.size.y - sy), TOL); - ASSERT_LT(fabsf(box.value - val), TOL) << box.value; - - ASSERT_LT(fabsf(box.centroid.x - cx), TOL); - ASSERT_LT(fabsf(box.centroid.y - cy), TOL); - } - - void test_corners(const std::vector & expect, const float TOL = 1.0E-6F) const - { - for (uint32_t idx = 0U; idx < 4U; ++idx) { - bool found = false; - for (auto & p : expect) { - if (fabsf(x_(p) - box.corners[idx].x) < TOL && fabsf(y_(p) - box.corners[idx].y) < TOL) { - found = true; - break; - } - } - ASSERT_TRUE(found) << idx << ": " << box.corners[idx].x << ", " << box.corners[idx].y; - } - } - - /// \brief th_deg - phi_deg, normalized to +/- 180 deg - float32_t angle_distance_deg(const float th_deg, const float phi_deg) const - { - return fmodf((th_deg - phi_deg) + 540.0F, 360.0F) - 180.0F; - } - - /// \brief converts a radian value to a degree value - float32_t rad2deg(const float rad_val) const { return rad_val * 57.2958F; } - - void test_orientation(const float ref_angle_deg, const float TOL = 1.0E-4F) const - { - bool found = false; - const float angle_deg = rad2deg(2.0F * asinf(box.orientation.z)); - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 90.0F)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 180.0F)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 270.0F)) < TOL; - ASSERT_TRUE(found) << angle_deg; - } -}; // BoxTest - -// Instantiate tests for given types, add more types here as they are used -using PointTypesBoundingBox = - ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(BoxTest, PointTypesBoundingBox, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -// TODO(c.ho) consider typed and parameterized tests: -// https://stackoverflow.com/questions/3258230/passing-a-typename-and-string-to-parameterized-test-using-google-test - -/////////////////////////////////////////// -TYPED_TEST(BoxTest, PointSegmentDistance) -{ - using autoware::common::geometry::closest_segment_point_2d; - using autoware::common::geometry::point_line_segment_distance_2d; - // normal case - TypeParam p = this->make(-1.0F, 0.0F); - TypeParam q = this->make(-1.0F, 2.0F); - TypeParam r = this->make(1.0F, 1.0F); - TypeParam t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -1.0F); - ASSERT_FLOAT_EQ(y_(t), 1.0F); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 2.0F); - // boundary case - p = this->make(1.0F, 0.0F); - q = this->make(-2.0F, 0.0F); - r = this->make(-5.0F, 0.0F); - t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -2.0F); - ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 3.0F); - // singular case - p = this->make(1.0F, 5.0F); - q = this->make(1.0F, 5.0F); - r = this->make(1.0F, 1.0F); - t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), 1.0F); - ASSERT_FLOAT_EQ(y_(t), 5.0F); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 4.0F); -} - -TYPED_TEST(BoxTest, ClosestPointOnLine) -{ - using autoware::common::geometry::closest_line_point_2d; - // normal case - TypeParam p = this->make(-1.0F, 0.0F); - TypeParam q = this->make(-1.0F, 2.0F); - TypeParam r = this->make(1.0F, 1.0F); - TypeParam t = closest_line_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -1.0F); - ASSERT_FLOAT_EQ(y_(t), 1.0F); - // out-of-boundary case - p = this->make(1.0F, 0.0F); - q = this->make(-2.0F, 0.0F); - r = this->make(-5.0F, 0.0F); - t = closest_line_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -5.0F); - ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); - // singular case - p = this->make(1.0F, 5.0F); - q = this->make(1.0F, 5.0F); - r = this->make(1.0F, 1.0F); - EXPECT_THROW(t = closest_line_point_2d(p, q, r), std::runtime_error); -} - -TYPED_TEST(BoxTest, Basic) -{ - const std::vector data{ - {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}}; - this->points.insert(this->points.begin(), data.begin(), data.end()); - - this->minimum_area_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); - this->test_corners(data); - this->test_orientation(0.0F); - - this->minimum_perimeter_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 2.0F); - this->test_corners(data); - this->test_orientation(0.0F); -} - -// -TYPED_TEST(BoxTest, OrientedTriangle) -{ - this->points.insert(this->points.begin(), {this->make(5, 5), this->make(7, 7), this->make(5, 7)}); - - this->minimum_area_bounding_box(); - - this->check(5.5F, 6.5F, sqrtf(2.0F), 2.0F * sqrtf(2.0F), 4.0F); - this->test_corners({this->make(5, 5), this->make(7, 7), this->make(4, 6), this->make(6, 8)}); - this->test_orientation(45.0F); - - this->minimum_perimeter_bounding_box(); - - this->check(6.0F, 6.0F, 2.0F, 2.0F, 4.0F); - this->test_corners({this->make(5, 5), this->make(7, 7), this->make(5, 7), this->make(7, 5)}); - this->test_orientation(0.0F); -} -// -TYPED_TEST(BoxTest, Hull) -{ - const uint32_t FUZZ_SIZE = 1024U; - const float dx = 9.0F; - const float dy = 15.0F; - const float rx = 10.0F; - const float ry = 5.0F; - const float dth = 0.0F; - - ASSERT_EQ(FUZZ_SIZE % 4U, 0U); - - // fuzz part 1 - for (uint32_t idx = 0U; idx < FUZZ_SIZE; ++idx) { - const float th = ((idx * autoware::common::types::TAU) / FUZZ_SIZE) + dth; - this->points.push_back(this->make(rx * cosf(th) + dx, ry * sinf(th) + dy)); - } - - this->minimum_area_bounding_box(); - - // allow 10cm = 2% of size for tolerance - const float TOL_M = 0.1F; - ASSERT_LT(fabsf(this->box.centroid.x - dx), TOL_M); - ASSERT_LT(fabsf(this->box.centroid.y - dy), TOL_M); - - this->test_corners( - {this->make(dx + rx, dy + ry), this->make(dx - rx, dy + ry), this->make(dx - rx, dy - ry), - this->make(dx + rx, dy - ry)}, - TOL_M); - - this->test_orientation(this->rad2deg(dth), 1.0F); - // allow 1 degree of tolerance - - ASSERT_LT(fabsf(this->box.size.y - 2.0F * rx), TOL_M); - ASSERT_LT(fabsf(this->box.size.x - 2.0F * ry), TOL_M); - ASSERT_FLOAT_EQ(this->box.value, this->box.size.x * this->box.size.y); -} - -// -TYPED_TEST(BoxTest, Collinear) -{ - this->points.insert( - this->points.begin(), {this->make(-2, -2), this->make(-3, -2), this->make(-4, -2), - this->make(-2, -4), this->make(-3, -4), this->make(-4, -4), - this->make(-2, -3), this->make(-2, -3), this->make(-2, -4)}); - - this->minimum_area_bounding_box(); - - this->check(-3.0F, -3.0F, 2.0F, 2.0F, 4.0F); - this->test_corners( - {this->make(-2, -2), this->make(-2, -4), this->make(-4, -4), this->make(-4, -2)}); - this->test_orientation(0.0F); -} - -// -TYPED_TEST(BoxTest, Line1) -{ - this->points.insert( - this->points.begin(), {this->make(-4, 3), this->make(-8, 6), this->make(-12, 9), - this->make(-16, 12), this->make(-20, 15), this->make(-24, 18), - this->make(-28, 21), this->make(-32, 24), this->make(-36, 27)}); - - this->minimum_area_bounding_box(); - - this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); - this->test_orientation(this->rad2deg(atan2f(3, -4))); - this->test_corners( - {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)}); - - this->minimum_perimeter_bounding_box(); - - this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 40.00001F); - this->test_orientation(this->rad2deg(atan2f(3, -4))); - this->test_corners( - {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)}); -} - -// -TYPED_TEST(BoxTest, Line2) -{ - this->points.insert( - this->points.begin(), - {this->make(4, 0), this->make(8, 0), this->make(12, 0), this->make(16, 0), this->make(20, 0), - this->make(24, 0), this->make(28, 0), this->make(32, 0), this->make(36, 0)}); - - this->minimum_area_bounding_box(); - - this->check(20.0F, 0.0F, 1.0E-6F, 32.0F, 3.2E-5F); - this->test_orientation(0.0F); - this->test_corners({this->make(4, 0), this->make(36, 0), this->make(4, 0), this->make(36, 0)}); -} - -// -TYPED_TEST(BoxTest, Line3) -{ - this->points.insert( - this->points.begin(), - {this->make(4, 3), this->make(8, 6), this->make(12, 9), this->make(16, 12), this->make(20, 15), - this->make(24, 18), this->make(28, 21), this->make(32, 24), this->make(36, 27)}); - - this->minimum_area_bounding_box(); - - this->check(20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); - this->test_orientation(this->rad2deg(atan2f(3, 4))); - this->test_corners({this->make(4, 3), this->make(36, 27), this->make(4, 3), this->make(36, 27)}); -} - -/* _ - /_/ <-- first guess is suboptimal - -*/ -TYPED_TEST(BoxTest, SuboptimalInit) -{ - this->points.insert( - this->points.begin(), - {this->make(8, 15), this->make(17, 0), this->make(0, 0), this->make(25, 15)}); - - this->minimum_area_bounding_box(); - - this->check(12.5F, 7.5F, 15.0F, 25.0F, 375.0F); - this->test_orientation(this->rad2deg(atan2f(15, 8))); - // these are approximate. - this->test_corners( - {this->make(0, 0), this->make(25, 15), this->make(11.7647F, 22.0588F), - this->make(13.2353F, -7.05882F)}, - 0.1F); -} - -// -TYPED_TEST(BoxTest, Centered) -{ - this->points.insert( - this->points.begin(), - {this->make(-1, 0), this->make(1, 0), this->make(0, -1), this->make(0, 1)}); - - this->minimum_area_bounding_box(); - - this->check(0.0F, 0.0F, sqrtf(2.0F), sqrtf(2.0F), 2.0F); - this->test_orientation(45.0F); - this->test_corners({this->make(-1, 0), this->make(1, 0), this->make(0, 1), this->make(0, -1)}); -} - -// convex_hull is imperfect in this case, check if this can save the day -TYPED_TEST(BoxTest, OverlappingPoints) -{ - this->points.insert( - this->points.begin(), {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1), - this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}); - - this->minimum_area_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); - this->test_orientation(0.0F); - this->test_corners({this->make(0, 0), this->make(1, 0), this->make(0, 1), this->make(1, 1)}); -} - -// check that minimum perimeter box is different from minimum area box -TYPED_TEST(BoxTest, Perimeter) -{ - this->points.insert( - this->points.begin(), {this->make(0, 0), this->make(0, 1), this->make(0, 2), this->make(0, 3), - this->make(0, 4), this->make(1, -0.1), // small fudge to force diagonal - this->make(2, 0), this->make(3, 0)}); - - this->minimum_area_bounding_box(); - - this->check(0.54F, 1.28F, 5.0F, 12.0F / 5.0F, 12.0F); - this->test_orientation(-53.13F, 0.001F); - this->test_corners( - {this->make(3, 0), this->make(0, 4), this->make(-1.92, 2.56), this->make(1.08, -1.44)}); - - // eigenbox should produce AABB TODO(c.ho) - // compute minimum perimeter box - this->minimum_perimeter_bounding_box(); - this->check(1.5F, 1.95F, 4.1F, 3.0F, 7.1F); - // perimeter for first box would be 14.8 - this->test_orientation(0.0F, 0.001F); - this->test_corners( - {this->make(3, -0.1), this->make(0, 4), this->make(3, 4), this->make(0, -0.1)}); -} - -// bounding box is diagonal on an L -TYPED_TEST(BoxTest, Eigenbox1) -{ - std::vector v{this->make(0, 0), this->make(0, 1), - this->make(-1, 2), // small fudge to force diagonal - this->make(0, 3), this->make(0, 4), - this->make(1, 0), this->make(2, -1), // small fudge to force diagonal - this->make(3, 0), this->make(4, 0)}; - this->points.insert(this->points.begin(), v.begin(), v.end()); - - // rotating calipers should produce a diagonal box - this->minimum_area_bounding_box(); - const float r = 4.0F; - - this->check(1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * r); - const std::vector diag_corners{ - this->make(4, 0), this->make(0, 4), this->make(-2, 2), this->make(2, -2)}; - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - - //// perimeter should also produce diagonal //// - this->minimum_perimeter_bounding_box(); - this->check( - 1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * (sqrtf(2.0F) + (1.0F / sqrtf(2.0F)))); - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - //// eigenbox should also produce aabb //// - this->eigenbox(v.begin(), v.end()); - // TODO(c.ho) don't care about value.. - this->check(1.0F, 1.0F, r * sqrtf(2.0F), r / sqrtf(2.0F), 4.375F); - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - //// Lfit should produce aabb //// - this->lfit_bounding_box_2d(v.begin(), v.end()); - - this->test_corners( - {this->make(4, -1), this->make(-1, 4), this->make(4, 4), this->make(-1, -1)}, 0.25F); - this->test_orientation(0.0F, 3.0F); -} - -// same as above test, just rotated -TYPED_TEST(BoxTest, Eigenbox2) -{ - std::vector v{this->make(0, 0), this->make(1, 1), - this->make(1, 2), // small fudge to force diagonal - this->make(3, 3), this->make(4, 4), - this->make(1, -1), this->make(1, -2), // small fudge to force diagonal - this->make(3, -3), this->make(4, -4)}; - this->points.insert(this->points.begin(), v.begin(), v.end()); - - const std::vector diag_corners{ - this->make(4, 4), this->make(0, 4), this->make(0, -4), this->make(4, -4)}; - // rotating calipers should produce a aabb - this->minimum_area_bounding_box(); - this->check(2.0F, 0.0F, 8, 4, 32); - this->test_corners(diag_corners); - this->test_orientation(0.0F, 0.001F); - - //// perimeter should also produce aabb //// - this->minimum_perimeter_bounding_box(); - this->check(2.0F, 0.0F, 8, 4, 12); - this->test_corners(diag_corners); - - //// eigenbox should also produce obb //// - this->eigenbox(v.begin(), v.end()); - this->test_orientation(0.0F, 0.001F); - this->test_corners(diag_corners); - //// Lfit should produce obb //// - this->lfit_bounding_box_2d(v.begin(), v.end()); - this->test_corners( - {this->make(3.5, 4.5), this->make(-1, 0), this->make(3.5, -4.5), this->make(8, 0)}, 0.2F); - this->test_orientation(45.0F, 2.0F); -} -// line case for eigenbox -TYPED_TEST(BoxTest, Eigenbox3) -{ - // horizontal line with some noise - std::vector v{this->make(0, 0.00), this->make(1, -0.01), this->make(3, 0.02), - this->make(3, 0.03), this->make(4, -0.04), this->make(-1, 0.01), - this->make(-2, -0.02), this->make(-3, -0.03), this->make(-4, 0.04)}; - this->lfit_bounding_box_2d(v.begin(), v.end()); - this->test_corners( - {this->make(-4, -0.04), this->make(-4, 0.04), this->make(4, -0.04), this->make(4, 0.04)}, 0.2F); - this->test_orientation(0.0F, 1.0F); -} - -// bad case: causes intersection2d to fail -// See https://gitlab.apex.ai/ApexAI/grand_central/issues/2862#note_156875 for more failure cases -TYPED_TEST(BoxTest, IntersectFail) -{ - std::vector vals{ - this->make(-13.9, 0.100006), this->make(-14.1, 0.100006), this->make(-13.9, 0.300003), - this->make(-14.1, 0.300003), this->make(-13.9, 0.5), this->make(-14.1, 0.5), - this->make(-13.9, 0.699997), this->make(-14.1, 0.699997), this->make(-14.3, 0.699997)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); - vals = {this->make(-2.1, 10.1), this->make(-1.89999, 10.1), this->make(-1.89999, 10.3), - this->make(-1.7, 10.3), this->make(-1.5, 10.3), this->make(-1.3, 10.3), - this->make(-0.5, 10.3), this->make(-0.300003, 10.3), this->make(-0.0999908, 10.3), - this->make(0.699997, 10.3), this->make(0.900009, 10.3), this->make(1.3, 10.3), - this->make(1.7, 10.3)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); - vals = {this->make(2.7, -5.5), this->make(2.7, -5.3), this->make(2.7, -5.1), - this->make(2.7, -4.9), this->make(2.7, -4.7), this->make(2.7, -4.5), - this->make(2.7, -4.3), this->make(2.7, -4.1), this->make(2.7, -3.9)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); -} -/// Handle slight floating point underflow case -// Note: raw discriminant checks are disabled because they don't work. I suspect this is due to -// tiny differences in floating point math when using our compile flags -TYPED_TEST(BoxTest, EigUnderflow) -{ - using autoware::common::geometry::bounding_box::details::Covariance2d; - // auto discriminant = [](const Covariance2d cov) -> float32_t { - // // duplicated raw math - // const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; - // const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); - // return (tr_2 * tr_2) - det; - // }; - TypeParam u, v; - const Covariance2d c1{0.0300002, 0.0300002, 5.46677e-08, 0U}; - // EXPECT_LT(discriminant(c1), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c1, u, v)); - const Covariance2d c2{0.034286, 0.0342847, 2.12874e-09, 0U}; - // EXPECT_LT(discriminant(c2), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c2, u, v)); - const Covariance2d c3{0.0300002, 0.0300002, -2.84987e-08, 0U}; - // EXPECT_LT(discriminant(c3), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c3, u, v)); - const Covariance2d c4{0.0300004, 0.0300002, 3.84156e-08, 0U}; - // EXPECT_LT(discriminant(c4), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c4, u, v)); - const Covariance2d c5{0.0300014, 0.0300014, -7.45058e-09, 0U}; - // EXPECT_LT(discriminant(c5), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c5, u, v)); - const Covariance2d c6{0.0400004, 0.0400002, 3.28503e-08, 0U}; - // EXPECT_LT(discriminant(c6), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c6, u, v)); -} - -//////////////////////////////////////////////// - -#endif // TEST_BOUNDING_BOX_HPP_ diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp deleted file mode 100644 index fc2d97c257b95..0000000000000 --- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp +++ /dev/null @@ -1,260 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef TEST_SPATIAL_HASH_HPP_ -#define TEST_SPATIAL_HASH_HPP_ - -#include "autoware_auto_geometry/spatial_hash.hpp" - -#include - -#include -#include - -using autoware::common::geometry::spatial_hash::Config2d; -using autoware::common::geometry::spatial_hash::Config3d; -using autoware::common::geometry::spatial_hash::SpatialHash; -using autoware::common::geometry::spatial_hash::SpatialHash2d; -using autoware::common::geometry::spatial_hash::SpatialHash3d; -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedSpatialHashTest : public ::testing::Test -{ -public: - TypedSpatialHashTest() : ref(), EPS(0.001F) - { - ref.x = 0.0F; - ref.y = 0.0F; - ref.z = 0.0F; - } - -protected: - template - void add_points( - SpatialHash & hash, const uint32_t points_per_ring, const uint32_t num_rings, - const float32_t dr, const float32_t dx = 0.0F, const float32_t dy = 0.0F) - { - const float32_t dth = 2.0F * 3.14159F / points_per_ring; - - // insert - float32_t r = dr; - for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { - float32_t th = 0.0F; - for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { - PointT pt; - pt.x = r * cosf(th) + dx; - pt.y = r * sinf(th) + dy; - pt.z = 0.0F; - hash.insert(pt); - th += dth; - } - r += dr; - } - } - PointT ref; - const float32_t EPS; -}; // SpatialHash -// test struct - -// Instantiate tests for given types, add more types here as they are used -using PointTypesSpatialHash = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedSpatialHashTest, PointTypesSpatialHash, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -/////////////////////////////////////////////////////////////// -// TODO(christopher.ho) helper functions to simplify this stuff -/// all points in one bin -TYPED_TEST(TypedSpatialHashTest, OneBin) -{ - using PointT = TypeParam; - const float32_t dr = 1.0F; - Config2d cfg{-10.0F, 10.0F, -10.0F, 10.0F, 3.0F, 1024U}; - SpatialHash2d hash{cfg}; - - // build concentric rings around origin - const uint32_t PTS_PER_RING = 10U; - const uint32_t NUM_RINGS = 1U; - this->add_points(hash, PTS_PER_RING, NUM_RINGS, dr); - - // loop through all points - float r = dr - this->EPS; - for (uint32_t rdx = 0U; rdx < NUM_RINGS + 1U; ++rdx) { - const uint32_t n_pts = rdx * PTS_PER_RING; - const auto & neighbors = hash.near(this->ref, r); - uint32_t points_seen = 0U; - for (const auto & itd : neighbors) { - const PointT & pt = itd; - const float dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); - ASSERT_LT(dist, r); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - ASSERT_EQ(points_seen, n_pts); - r += dr; - // Make sure statistics are consistent - EXPECT_EQ(hash.bins_hit(), 9U * (1U + rdx)); - EXPECT_EQ(hash.neighbors_found(), rdx * PTS_PER_RING); - } - // check iterators etc. - uint32_t count = 0U; - for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { - // TODO(c.ho) check uniqueness of stuff - ++count; - } - EXPECT_EQ(PTS_PER_RING, count); - hash.clear(); - EXPECT_EQ(hash.size(), 0U); - EXPECT_TRUE(hash.empty()); - count = 0U; - for (auto it : hash) { - // TODO(c.ho) check uniqueness of stuff - (void)it; - ++count; - } - EXPECT_EQ(count, 0U); -} -/// test out of bounds points -TYPED_TEST(TypedSpatialHashTest, Oob) -{ - using PointT = TypeParam; - const float32_t dr = 20.0F; - Config2d cfg{-2.0F, 2.0F, -2.0F, 2.0F, dr + this->EPS, 1024U}; - SpatialHash2d hash{cfg}; - - // build concentric rings around origin - const uint32_t PTS_PER_RING = 12U; - this->add_points(hash, PTS_PER_RING, 1U, dr); - - // loop through all points - float32_t r = dr + this->EPS; - const uint32_t n_pts = PTS_PER_RING; - const auto & neighbors = hash.near(this->ref, r); - uint32_t points_seen = 0U; - for (const auto itd : neighbors) { - const PointT & pt = itd; - const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); - ASSERT_LT(dist, r); - ASSERT_GT(dist, 10.0F * sqrtf(2.0F)); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - - ASSERT_EQ(points_seen, n_pts); -} -// 3d test case -TYPED_TEST(TypedSpatialHashTest, 3d) -{ - using PointT = TypeParam; - Config3d cfg{-30.0F, 30.0F, -30.0F, 30.0F, -30.0F, 30.0F, 30.0F, 1024U}; - SpatialHash3d hash{cfg}; - EXPECT_TRUE(hash.empty()); - - // build concentric rings around origin - const uint32_t points_per_ring = 32U; - const uint32_t num_rings = 5U; - const float32_t dth = 2.0F * 3.14159F / points_per_ring; - std::vector pts{}; - - // insert - const float32_t r = 10.0F; - float32_t phi = 0.0f; - for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { - float32_t th = 0.0F; - for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { - PointT pt; - pt.x = r * cosf(th) * cosf(phi); - pt.y = r * sinf(th) * cosf(phi); - pt.z = r * sinf(phi); - pts.push_back(pt); - th += dth; - } - hash.insert(pts.begin(), pts.end()); - pts.clear(); - phi += 1.0f; - } - EXPECT_FALSE(hash.empty()); - EXPECT_EQ(hash.size(), num_rings * points_per_ring); - - // loop through all points - const uint32_t n_pts = num_rings * points_per_ring; - const auto & neighbors = hash.near(this->ref, r + this->EPS); - uint32_t points_seen = 0U; - for (const auto & itd : neighbors) { - const PointT & pt = itd; - const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y) + (pt.z * pt.z)); - ASSERT_LT(dist, r + this->EPS); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - - ASSERT_EQ(points_seen, n_pts); - - // check iterators etc. - uint32_t count = 0U; - for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { - // TODO(c.ho) check uniqueness of stuff - ++count; - } - EXPECT_EQ(n_pts, count); - hash.clear(); - EXPECT_EQ(hash.size(), 0U); - EXPECT_TRUE(hash.empty()); - count = 0U; - for (auto it : hash) { - // TODO(c.ho) check uniqueness of stuff - (void)it; - ++count; - } - EXPECT_EQ(count, 0U); -} - -/// edge cases -TEST(SpatialHashConfig, BadCases) -{ - // negative side length - EXPECT_THROW(Config2d({-30.0F, 30.0F, -30.0F, 30.0F, -1.0F, 1024U}), std::domain_error); - // min_x >= max_x - EXPECT_THROW(Config2d({31.0F, 30.0F, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - // min_y >= max_y - EXPECT_THROW(Config2d({-30.0F, 30.0F, 30.1F, 30.0F, 1.0F, 1024U}), std::domain_error); - // min_z >= max_z - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -30.0F, 30.0F, 31.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - // floating point limit - constexpr float32_t max_float = std::numeric_limits::max(); - EXPECT_THROW(Config2d({-max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), - std::domain_error); - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -30.0F, 30.0F, -max_float, max_float, 1.0F, 1024U}), - std::domain_error); - // y would overflow - // constexpr float32_t big_float = - // static_cast(std::numeric_limits::max() / 4UL); - // EXPECT_THROW(Config({-big_float, big_float, -big_float, big_float, 0.001F, 1024U}), - // std::domain_error); - // z would overflow - // EXPECT_THROW( - // Config3d({-30.0F, 30.0F, -99999.0F, 99999.0F, -99999.0F, 99999.0F, 0.001F, 1024U}), - // std::domain_error); - // TODO(c.ho) re-enable test when we can actually check unsigned integer multiplication overflow -} -#endif // TEST_SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp deleted file mode 100644 index 81865656c55b5..0000000000000 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/lookup_table.hpp" - -#include - -#include - -#include -#include - -using autoware::common::helper_functions::interpolate; -using autoware::common::helper_functions::lookup_1d; -using autoware::common::helper_functions::LookupTable1D; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class BadCase : public ::testing::Test -{ -}; - -using BadTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(BadCase, BadTypes, ); - -// Empty domain/range -TYPED_TEST(BadCase, Empty) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({}, {}, T{}), std::domain_error); -} - -// Unequal domain/range -TYPED_TEST(BadCase, UnequalDomain) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({T{1}, T{2}}, {T{1}}, T{}), std::domain_error); -} - -// Not sorted domain -TYPED_TEST(BadCase, DomainNotSorted) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({T{2}, T{1}}, {T{1}, T{2}}, T{}), std::domain_error); -} - -//////////////////////////////////////////////////////////////////////////////// - -template -class SanityCheck : public ::testing::Test -{ -public: - using T = Type; - - void SetUp() override - { - domain_ = std::vector{T{1}, T{3}, T{5}}; - range_ = std::vector{T{2}, T{4}, T{0}}; - ASSERT_NO_THROW(table_ = std::make_unique>(domain_, range_)); - } - - bool not_in_domain(const T bad_value) const noexcept - { - return std::find(domain_.begin(), domain_.end(), bad_value) == domain_.end(); - } - - void check(const T expected, const T actual) const noexcept - { - if (std::is_floating_point::value) { - EXPECT_FLOAT_EQ(actual, expected); - } else { - EXPECT_EQ(actual, expected); - } - } - -protected: - std::vector domain_{}; - std::vector range_{}; - std::unique_ptr> table_{}; -}; - -using NormalTypes = ::testing::Types; -TYPED_TEST_SUITE(SanityCheck, NormalTypes, ); - -TYPED_TEST(SanityCheck, Exact) -{ - const auto x = this->domain_[1U]; - const auto result = this->table_->lookup(x); - ASSERT_FALSE(this->not_in_domain(x)); - this->check(result, this->range_[1U]); -} - -TYPED_TEST(SanityCheck, Interpolation) -{ - const auto x = TypeParam{2}; - // Assert x is not identically in domain_ - ASSERT_TRUE(this->not_in_domain(x)); - const auto result = this->table_->lookup(x); - this->check(result, TypeParam{3}); -} - -TYPED_TEST(SanityCheck, AboveRange) -{ - const auto x = TypeParam{999999}; - ASSERT_GT(x, this->domain_.back()); // domain is known to be sorted - const auto result = this->table_->lookup(x); - this->check(result, this->range_.back()); -} - -TYPED_TEST(SanityCheck, BelowRange) -{ - const auto x = TypeParam{0}; - ASSERT_LT(x, this->domain_.front()); // domain is known to be sorted - const auto result = this->table_->lookup(x); - this->check(result, this->range_.front()); -} - -TEST(LookupTableHelpers, Interpolate) -{ - { - const auto scaling = 0.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); - } - - { - const auto scaling = 1.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); - } - - { - const auto scaling = -1.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); - } - - { - const auto scaling = 2.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); - } - - { - const auto scaling = 0.75f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.75f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.125f); - } -} - -// TODO(c.ho) check with more interesting functions diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp deleted file mode 100644 index bc9c28682ed44..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_area.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// Copyright 2021 Apex.AI, 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -#include - -#include -#include -#include - -template -class AreaTest : public ::testing::Test -{ -protected: - DataStructure data_{}; - using Point = typename DataStructure::value_type; - using Real = decltype(autoware::common::geometry::point_adapter::x_(std::declval())); - - auto area() { return autoware::common::geometry::area_checked_2d(data_.begin(), data_.end()); } - - void add_point(Real x, Real y) - { - namespace pa = autoware::common::geometry::point_adapter; - Point p{}; - pa::xr_(p) = x; - pa::yr_(p) = y; - (void)data_.insert(data_.end(), p); - } -}; - -// Data structures to test... -template -using TestTypes_ = ::testing::Types..., std::list...>; -// ... and point types to test -using TestTypes = TestTypes_; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(AreaTest, TestTypes, ); - -// The empty set has zero area -TYPED_TEST(AreaTest, DegenerateZero) -{ - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// An individual point has zero area -TYPED_TEST(AreaTest, DegenerateOne) -{ - this->add_point(0.0, 0.0); - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// An line segment has zero area -TYPED_TEST(AreaTest, DegenerateTwo) -{ - this->add_point(1.0, -1.0); - this->add_point(-3.0, 2.0); - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// Simple triangle -TYPED_TEST(AreaTest, Triangle) -{ - this->add_point(1.0, 0.0); - this->add_point(3.0, 0.0); // 2.0 wide - this->add_point(2.0, 2.0); // 2.0 tall - EXPECT_FLOAT_EQ(2.0, this->area()); // A = (1/2) * b * h -} - -// Rectangle is easy to do computational -TYPED_TEST(AreaTest, Rectangle) -{ - this->add_point(-5.0, -5.0); - this->add_point(-2.0, -5.0); // L = 3 - this->add_point(-2.0, -1.0); // H = 4 - this->add_point(-5.0, -1.0); - EXPECT_FLOAT_EQ(12.0, this->area()); // A = b * h -} - -// Parallelogram is slightly less trivial than a rectangle -TYPED_TEST(AreaTest, Parallelogram) -{ - this->add_point(-5.0, 1.0); - this->add_point(-2.0, 1.0); // L = 3 - this->add_point(-1.0, 3.0); // H = 2 - this->add_point(-4.0, 3.0); - EXPECT_FLOAT_EQ(6.0, this->area()); // A = b * h -} - -// Octagon is analytical and reasonably easy to build -TYPED_TEST(AreaTest, Octagon) -{ - const auto sq2 = std::sqrt(2.0); - const auto a = 1.0; - const auto a2 = a / 2.0; - const auto b = (a + sq2) / 2.0; - this->add_point(-a2, -b); - this->add_point(a2, -b); - this->add_point(b, -a2); - this->add_point(b, a2); - this->add_point(a2, b); - this->add_point(-a2, b); - this->add_point(-b, a2); - this->add_point(-b, -a2); - const auto expect = (2.0 * (1.0 + sq2)) * (a * a); - EXPECT_FLOAT_EQ(expect, this->area()); // A = b * h -} - -// Bad case -TYPED_TEST(AreaTest, NotCcw) -{ - this->add_point(0.0, 0.0); - this->add_point(1.0, 1.0); - this->add_point(1.0, 0.0); - this->add_point(2.0, 1.0); - EXPECT_THROW(this->area(), std::domain_error); -} diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp deleted file mode 100644 index baf6967edd47e..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_common_2d.cpp +++ /dev/null @@ -1,197 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/common_2d.hpp" - -#include -#include - -#include - -#include -#include - -using autoware::common::geometry::point_adapter::xr_; -using autoware::common::geometry::point_adapter::yr_; - -// Helper function for adding new points -template -T make_points(const float x, const float y) -{ - T ret; - xr_(ret) = x; - yr_(ret) = y; - return ret; -} - -// PointTypes to be tested -using PointTypes = - ::testing::Types; - -// Wrapper function for stubbing output of -// autoware::common::geometry::check_point_position_to_line_2d -template -int point_position_checker(const T & p1, const T & p2, const T & q) -{ - auto result = autoware::common::geometry::check_point_position_to_line_2d(p1, p2, q); - if (result > 0.0F) { - return 1; - } else if (result < 0.0F) { - return -1; - } - return result; -} - -// Templated struct defining the function parameters for -// autoware::common::geometry::check_point_position_to_line_2d and input_output vector for -// assertion of return values from the function -template -struct PointPositionToLine : public ::testing::Test -{ - struct Parameters - { - T p1; - T p2; - T q; - }; - static std::vector> input_output; -}; - -TYPED_TEST_SUITE_P(PointPositionToLine); - -template -std::vector::Parameters, int>> - PointPositionToLine::input_output{ - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(1.0F, 5.0F)}, -1}, - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-1.0F, 0.5F)}, 1}, - // Check point on the line - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-2.0F, 2.0F)}, 0}, - }; - -TYPED_TEST_P(PointPositionToLine, PointPositionToLineTest) -{ - for (size_t i = 0; i < PointPositionToLine::input_output.size(); ++i) { - const auto & input = PointPositionToLine::input_output[i].first; - EXPECT_EQ( - point_position_checker(input.p1, input.p2, input.q), - PointPositionToLine::input_output[i].second) - << "Index " << i; - } -} - -REGISTER_TYPED_TEST_SUITE_P(PointPositionToLine, PointPositionToLineTest); -// cppcheck-suppress syntaxError -INSTANTIATE_TYPED_TEST_SUITE_P(Test, PointPositionToLine, PointTypes, ); - -///////////////////////////////////////////////////////////////////////////////////// - -// Templated struct defining the function parameters for -// autoware::common::geometry::is_point_inside_polygon_2d and input_output vector for -// assertion of return values from the function -template -struct InsidePolygon : public ::testing::Test -{ - struct Parameters - { - std::vector polygon; - T q; - }; - static std::vector> input_output; -}; - -TYPED_TEST_SUITE_P(InsidePolygon); - -template -std::vector::Parameters, bool>> InsidePolygon::input_output{ - // point inside the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.F, 0.5F)}, - true}, - // point below the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 0.F)}, - false}, - // point above the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 1.75F)}, - false}, - // point on the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 0.5F)}, - true}, -}; - -TYPED_TEST_P(InsidePolygon, InsidePolygonTest) -{ - for (size_t i = 0; i < InsidePolygon::input_output.size(); ++i) { - const auto & input = InsidePolygon::input_output[i].first; - EXPECT_EQ( - autoware::common::geometry::is_point_inside_polygon_2d( - input.polygon.begin(), input.polygon.end(), input.q), - InsidePolygon::input_output[i].second) - << "Index " << i; - } -} - -REGISTER_TYPED_TEST_SUITE_P(InsidePolygon, InsidePolygonTest); -// cppcheck-suppress syntaxError -INSTANTIATE_TYPED_TEST_SUITE_P(Test, InsidePolygon, PointTypes, ); - -TEST(ordered_check, basic) -{ - // CW - std::vector points_list = { - make_points(8.0, 4.0), - make_points(9.0, 1.0), - make_points(3.0, 2.0), - make_points(2.0, 5.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // CCW - points_list = { - make_points(2.0, 5.0), - make_points(3.0, 2.0), - make_points(9.0, 1.0), - make_points(8.0, 4.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Unordered - points_list = { - make_points(2.0, 5.0), - make_points(3.0, 2.0), - make_points(8.0, 4.0), - make_points(9.0, 1.0)}; - EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Unordered - points_list = { - make_points(0.0, 0.0), - make_points(1.0, 1.0), - make_points(1.0, 0.0), - make_points(2.0, 1.0)}; - EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Collinearity - points_list = { - make_points(2.0, 2.0), - make_points(4.0, 4.0), - make_points(6.0, 6.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); -} diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp deleted file mode 100644 index 8b9bbce36c428..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp +++ /dev/null @@ -1,372 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" - -#include - -#include - -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedConvexHullTest : public ::testing::Test -{ -protected: - std::list list; - - typename std::list::const_iterator convex_hull() - { - const auto ret = autoware::common::geometry::convex_hull(list); - return ret; - } - - void check_hull( - const typename std::list::const_iterator last, const std::vector & expect, - const bool8_t strict = true) - { - uint32_t items = 0U; - for (auto & pt : expect) { - bool8_t found = false; - auto it = list.begin(); - while (it != last) { - constexpr float32_t TOL = 1.0E-6F; - if ( - fabsf(pt.x - it->x) <= TOL && fabsf(pt.y - it->y) <= TOL && - (fabsf(pt.z - it->z) <= TOL || !strict)) // TODO(@estive): z if only strict - { - found = true; - break; - } - ++it; - } - ASSERT_TRUE(found) << items; - ++items; - } - if (strict) { - ASSERT_EQ(items, expect.size()); - } - } - - PointT make(const float32_t x, const float32_t y, const float32_t z) - { - PointT ret; - ret.x = x; - ret.y = y; - ret.z = z; - return ret; - } -}; // class convex_hull_test - -// Instantiate tests for given types, add more types here as they are used -using PointTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedConvexHullTest, PointTypes, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -////////////////////////////////////////// - -/* - 3 - 2 -1 -*/ -TYPED_TEST(TypedConvexHullTest, Triangle) -{ - std::vector expect({this->make(1, 0, 0), this->make(3, 1, 0), this->make(2, 2, 0)}); - this->list.insert(this->list.begin(), expect.begin(), expect.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(this->list.size(), 3U); - // check order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, 3); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->x, 2); - ++it; // node 3 - ASSERT_EQ(it, last); -} -/* - 2 1 - -4 - 3 -*/ -// test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, Quadrilateral) -{ - std::vector expect( - {this->make(-1, -1, 1), this->make(-5, -1, 2), this->make(-2, -6, 3), this->make(-6, -5, 4)}); - this->list.insert(this->list.begin(), expect.begin(), expect.end()); - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(this->list.size(), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, -6); - ++it; // node 4 - ASSERT_FLOAT_EQ(it->x, -2); - ++it; // node 3 - ASSERT_FLOAT_EQ(it->x, -1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, -5); - ++it; // node 2 - ASSERT_EQ(it, last); -} - -// test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, QuadHull) -{ - std::vector data( - {this->make(1, 1, 1), this->make(5, 1, 2), this->make(2, 6, 3), this->make(3, 3, 4), - this->make(6, 5, 5)}); - std::vector expect{{data[0], data[1], data[2], data[4]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, 5); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->x, 6); - ++it; // node 4 - ASSERT_FLOAT_EQ(it->x, 2); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -// a ring plus a bunch of random stuff in the middle -TYPED_TEST(TypedConvexHullTest, Hull) -{ - const uint32_t HULL_SIZE = 13U; - const uint32_t FUZZ_SIZE = 50U; - const float32_t dth = 1.133729384F; // some weird irrational(ish) number - const float32_t r_hull = 20.0F; - const float32_t r_fuzz = 10.0F; - ASSERT_LT(r_fuzz, r_hull); - - std::vector hull; - - uint32_t hull_pts = 0U; - float32_t th = 0.0F; - // hull part 1 - for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++hull_pts; - } - - // fuzz part 1 - uint32_t fuzz_pts = 0U; - for (uint32_t idx = 0U; idx < FUZZ_SIZE / 2U; ++idx) { - const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++fuzz_pts; - } - - // hull part 2 - for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++hull_pts; - } - - // fuzz part 2 - for (uint32_t idx = 0U; idx < FUZZ_SIZE - fuzz_pts; ++idx) { - const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - } - - // hull part 3 - for (uint32_t idx = 0U; idx < HULL_SIZE - hull_pts; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - } - - const auto last = this->convex_hull(); - - this->check_hull(last, hull); - ASSERT_EQ(std::distance(this->list.cbegin(), last), HULL_SIZE); -} - -TYPED_TEST(TypedConvexHullTest, Collinear) -{ - std::vector data( - {this->make(0, 0, 1), this->make(1, 0, 2), this->make(2, 0, 3), this->make(0, 2, 4), - this->make(1, 2, 8), this->make(2, 2, 7), this->make(1, 0, 6), this->make(1, 2, 5), - this->make(1, 1, 0)}); - const std::vector expect{{data[0], data[2], data[3], data[5]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->z, 3); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->z, 7); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->z, 4); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -// degenerate cases -TYPED_TEST(TypedConvexHullTest, OverlappingPoints) -{ - std::vector data( - {this->make(3, -1, 1), this->make(4, -2, 2), this->make(5, -7, 3), this->make(4, -2, 4), - this->make(5, -7, 8), this->make(3, -1, 7), this->make(5, -7, 6), this->make(4, -2, 5), - this->make(3, -1, 0)}); - const std::vector expect{{data[0], data[1], data[2]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 3U); - this->check_hull(last, expect, false); -} - -TYPED_TEST(TypedConvexHullTest, Line) -{ - std::vector data( - {this->make(-3, 3, 1), this->make(-2, 2, 2), this->make(-1, 1, 3), this->make(-8, 8, 4), - this->make(-6, 6, 8), this->make(-4, 4, 7), this->make(-10, 10, 6), this->make(-12, 12, 5), - this->make(-11, 11, 0)}); - const std::vector expect{{data[2], data[7]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 2U); - this->check_hull(last, expect, false); - - // check for order: this part is a little loose - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 5); - ++it; // node 8 - ASSERT_FLOAT_EQ(it->z, 3); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -/* -1 - 4 - - 3 - 2 -*/ -TYPED_TEST(TypedConvexHullTest, LowerHull) -{ - const std::vector data({ - this->make(1, 3, 1), - this->make(2, -2, 2), - this->make(3, -1, 3), - this->make(4, 1, 4), - }); - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - this->check_hull(last, data); - - // check for order: this part is a little loose - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; - ASSERT_FLOAT_EQ(it->z, 2); - ++it; - ASSERT_FLOAT_EQ(it->z, 3); - ++it; - ASSERT_FLOAT_EQ(it->z, 4); - ++it; - ASSERT_EQ(it, last); -} - -// Ensure the leftmost item is properly shuffled -/* - 5 -1 6 - 2 4 - 3 -*/ -TYPED_TEST(TypedConvexHullTest, Root) -{ - const std::vector data({ - this->make(0, 0, 1), - this->make(1, -1, 2), - this->make(3, -2, 3), - this->make(4, 0, 4), - this->make(3, 1, 5), - this->make(1, 0, 6), - }); - const std::vector expect{{data[0], data[1], data[2], data[3], data[4]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 5); - this->check_hull(last, expect); - - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; - ASSERT_FLOAT_EQ(it->z, 2); - ++it; - ASSERT_FLOAT_EQ(it->z, 3); - ++it; - ASSERT_FLOAT_EQ(it->z, 4); - ++it; - ASSERT_FLOAT_EQ(it->z, 5); - ++it; - ASSERT_EQ(it, last); - EXPECT_NE(last, this->list.cend()); - EXPECT_EQ(last->z, 6); -} - -// TODO(c.ho) random input, fuzzing, stress tests diff --git a/common/autoware_auto_geometry/test/src/test_geometry.cpp b/common/autoware_auto_geometry/test/src/test_geometry.cpp deleted file mode 100644 index 0a7dd288d0d79..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_geometry.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2018 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "gtest/gtest.h" -#include "test_bounding_box.hpp" -#include "test_spatial_hash.hpp" - -int32_t main(int32_t argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp deleted file mode 100644 index 9477a83a488ed..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" -#include "autoware_auto_geometry/hull_pockets.hpp" - -#include - -#include - -#include -#include -#include -#include - -using autoware::common::geometry::point_adapter::x_; -using autoware::common::geometry::point_adapter::y_; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedHullPocketsTest : public ::testing::Test -{ -protected: - PointT make(const float32_t x, const float32_t y, const float32_t z) - { - PointT ret; - ret.x = x; - ret.y = y; - ret.z = z; - return ret; - } -}; // class TypedHullPocketsTest - -using PointTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedHullPocketsTest, PointTypes, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -// Inner test function, shared among all the tests -template -typename std::vector::value_type>> -compute_hull_and_pockets(const Iter polygon_start, const Iter polygon_end) -{ - auto convex_hull_list = - std::list::value_type>{polygon_start, polygon_end}; - const auto cvx_hull_end = autoware::common::geometry::convex_hull(convex_hull_list); - - const typename decltype(convex_hull_list)::const_iterator list_beginning = - convex_hull_list.begin(); - const auto pockets = autoware::common::geometry::hull_pockets( - polygon_start, polygon_end, list_beginning, cvx_hull_end); - - return pockets; -} - -// Test for a triangle - any triangle should really not result in any pockets. -TYPED_TEST(TypedHullPocketsTest, Triangle) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(1, 1, 0)}; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - ASSERT_EQ(pockets.size(), 0u); -} - -// Test for the use case: -// +--------------------+ -// | | -// | | -// | | -// | | -// | | -// | | -// +--------------------+ -// This should not result in any pockets. -TYPED_TEST(TypedHullPocketsTest, Box) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 1, 0), this->make(0, 1, 0)}; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - ASSERT_EQ(pockets.size(), 0u); -} - -// Test for the use case: -// +-----+ +-----+ -// / | | | -// / | | | -// + | | + -// | | | | -// | | | | -// | -------------- | -// | | -// | | -// | | -// | | -// +------------------------------+ -// This should come up with a single box on the top left. -TYPED_TEST(TypedHullPocketsTest, UShape) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(5, 0, 0), this->make(5, 4.5, 0), this->make(4, 5, 0), - this->make(4, 2, 0), this->make(2, 2, 0), this->make(2, 5, 0), this->make(0, 4.5, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 1u); - ASSERT_EQ(pockets[0].size(), 4u); - ASSERT_FLOAT_EQ(x_(pockets[0][0]), 4.0); - ASSERT_FLOAT_EQ(y_(pockets[0][0]), 5.0); - ASSERT_FLOAT_EQ(x_(pockets[0][1]), 4.0); - ASSERT_FLOAT_EQ(y_(pockets[0][1]), 2.0); -} - -// Test for the use case: -// +------+ -// | | -// | | -// | | -// +------------------+ +------+ -// | | -// | | -// | | -// +--------------------------------+ -// -// This should come up with two pockets, a triangle on the top left and one on the -// top right. -TYPED_TEST(TypedHullPocketsTest, TypicalGap) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(10, 0, 0), this->make(10, 2, 0), this->make(8, 2, 0), - this->make(8, 4, 0), this->make(6, 4, 0), this->make(6, 2, 0), this->make(0, 2, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 2u); - ASSERT_EQ(pockets[0].size(), 3u); - ASSERT_EQ(pockets[1].size(), 3u); - // TODO(s.me) check for correct pocket positioning -} - -// Test for the use case: -// -// +-----------------+ -// | | -// | | -// + | -// / | -// / | -// +-----------------+ -// -// This should come up with one pocket, in particular a pocket that contains -// the segment of the final to the first point. -TYPED_TEST(TypedHullPocketsTest, EndsInPocket) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 2, 0), - this->make(0, 2, 0), this->make(0.1, 1, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 1u); - ASSERT_EQ(pockets[0].size(), 3u); - // TODO(s.me) check for correct pocket positioning -} diff --git a/common/autoware_auto_geometry/test/src/test_intersection.cpp b/common/autoware_auto_geometry/test/src/test_intersection.cpp deleted file mode 100644 index 1036c69573c49..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_intersection.cpp +++ /dev/null @@ -1,155 +0,0 @@ -// Copyright 2021 Apex.AI, 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. - -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" -#include "autoware_auto_geometry/intersection.hpp" - -#include - -#include - -struct TestPoint -{ - autoware::common::types::float32_t x; - autoware::common::types::float32_t y; -}; - -struct IntersectionTestParams -{ - std::list polygon1_pts; - std::list polygon2_pts; - std::list expected_intersection_pts; -}; - -void order_ccw(std::list & points) -{ - const auto end_it = autoware::common::geometry::convex_hull(points); - ASSERT_EQ(end_it, points.end()); // Points should have represent a shape -} - -class IntersectionTest : public ::testing::TestWithParam -{ -}; - -TEST_P(IntersectionTest, Basic) -{ - const auto get_ordered_polygon = [](auto polygon) { - order_ccw(polygon); - return polygon; - }; - - const auto polygon1 = get_ordered_polygon(GetParam().polygon1_pts); - const auto polygon2 = get_ordered_polygon(GetParam().polygon2_pts); - const auto expected_intersection = get_ordered_polygon(GetParam().expected_intersection_pts); - - const auto result = autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); - - ASSERT_EQ(result.size(), expected_intersection.size()); - auto expected_shape_it = expected_intersection.begin(); - for (auto result_it = result.begin(); result_it != result.end(); ++result_it) { - EXPECT_FLOAT_EQ(result_it->x, expected_shape_it->x); - EXPECT_FLOAT_EQ(result_it->y, expected_shape_it->y); - ++expected_shape_it; - } -} - -INSTANTIATE_TEST_SUITE_P( - Basic, IntersectionTest, - ::testing::Values( - IntersectionTestParams{{}, {}, {}}, - IntersectionTestParams{// Partial intersection - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {15.0F, 5.0F}, {5.0F, 15.0F}, {15.0F, 15.0F}}, - {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}}, - // Full intersection with overlapping edges - // TODO(yunus.caliskan): enable after #1231 - // IntersectionTestParams{ - // {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, - // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, - // }, - IntersectionTestParams{ - // Fully contained - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, - }, - IntersectionTestParams{ - // Fully contained triangle - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, - }, - IntersectionTestParams{// Triangle rectangle intersection. - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 1.0F}, {5.0F, 9.0F}, {15.0F, 5.0F}}, - {{5.0F, 1.0F}, {5.0F, 9.0F}, {10.0F, 3.0F}, {10.0F, 7.0F}}}, - IntersectionTestParams{// No intersection - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{15.0F, 15.0F}, {20.0F, 15.0F}, {15.0F, 20.0F}, {20.0F, 20.0F}}, - {}} // cppcheck-suppress syntaxError - )); - -TEST(PolygonPointTest, Basic) -{ - GTEST_SKIP(); // TODO(yunus.caliskan): enable after #1231 - std::list polygon{{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}; - order_ccw(polygon); - EXPECT_FALSE(autoware::common::geometry::is_point_inside_polygon_2d( - polygon.begin(), polygon.end(), TestPoint{0.0F, 10.0F})); -} - -// IoU of two intersecting shapes: a pentagon and a square. The test includes pen and paper -// computations for the intermediate steps as assertions. -TEST(IoUTest, PentagonRectangleIntersection) -{ - std::list polygon1{ - {0.0F, 3.0F}, {3.0F, 4.0F}, {6.0F, 3.0F}, {4.0F, 1.0F}, {2.0F, 1.0F}}; - std::list polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}}; - - order_ccw(polygon1); - order_ccw(polygon2); - - const auto intersection = - autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); - const auto expected_intersection_area = - autoware::common::geometry::area_2d(intersection.begin(), intersection.end()); - ASSERT_FLOAT_EQ(expected_intersection_area, 1.5F); // Pen & paper proof. - - const auto expected_union_area = - autoware::common::geometry::area_2d(polygon1.begin(), polygon1.end()) + - autoware::common::geometry::area_2d(polygon2.begin(), polygon2.end()) - - expected_intersection_area; - ASSERT_FLOAT_EQ(expected_union_area, (11.0F + 4.0F - 1.5F)); // Pen & paper proof. - - const auto computed_iou = - autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2); - - EXPECT_FLOAT_EQ(computed_iou, expected_intersection_area / expected_union_area); -} - -// IoU of two non-intersecting rectangles. -TEST(IoUTest, NoIntersection) -{ - std::list polygon1{{0.0F, 0.0F}, {0.0F, 1.0F}, {1.0F, 1.0F}, {1.0F, 0.0F}}; - std::list polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}}; - - order_ccw(polygon1); - order_ccw(polygon2); - - EXPECT_FLOAT_EQ( - autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2), 0.0F); -} diff --git a/common/autoware_auto_geometry/test/src/test_interval.cpp b/common/autoware_auto_geometry/test/src/test_interval.cpp deleted file mode 100644 index 266ab123f5203..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_interval.cpp +++ /dev/null @@ -1,259 +0,0 @@ -// Copyright 2020 Mapless AI, Inc. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include -#include -#include - -using autoware::common::geometry::Interval; -using autoware::common::geometry::Interval_d; -using autoware::common::geometry::Interval_f; - -namespace -{ -const auto Inf = std::numeric_limits::infinity(); -const auto Min = std::numeric_limits::lowest(); -const auto Max = std::numeric_limits::max(); -const auto NaN = std::numeric_limits::quiet_NaN(); -const auto epsilon = 1e-5; -} // namespace - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, AbsEq) -{ - const auto i1 = Interval_d(-1.0, 1.0); - const auto i2 = Interval_d(-1.0 + 0.5 * epsilon, 1.0 + 0.5 * epsilon); - const auto shift = (2.0 * epsilon); - const auto i3 = Interval_d(-1.0 + shift, 1.0 + shift); - const auto i_empty = Interval_d(); - - EXPECT_TRUE(Interval_d::abs_eq(i1, i1, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i1, i2, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i2, i1, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i1, i3, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i3, i1, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i1, i_empty, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i_empty, i1, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i_empty, i_empty, epsilon)); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, IsSubsetEq) -{ - EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(-0.5, 0.5), Interval_d(-1.0, 1.0))); - EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(3.2, 4.2), Interval_d(3.2, 4.2))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(-3.0, -1.0), Interval_d(1.0, 3.0))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(1.0, 3.0), Interval_d(2.0, 4.0))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(), Interval_d())); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ClampTo) -{ - const auto i = Interval_d(-1.0, 1.0); - { - const auto val = 0.0; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, val); - } - - { - const auto val = -3.4; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, Interval_d::min(i)); - } - - { - const auto val = 2.7; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, Interval_d::max(i)); - } - - const auto val = 1.0; - const auto empty_interval = Interval_d(); - const auto projected = Interval_d::clamp_to(empty_interval, val); - EXPECT_TRUE(std::isnan(projected)); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Comparisons) -{ - { - const auto i1 = Interval_d(0.25, 1); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(-0.25, 0.5); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 0.5); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 1); - const auto i2 = Interval_d(0, 1); - EXPECT_TRUE((i1 == i2)); - EXPECT_FALSE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 1); - const auto i2 = Interval_d(); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(); - EXPECT_TRUE((i1 == i2)); - EXPECT_FALSE((i1 != i2)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Contains) -{ - { - const auto i = Interval_d(); - EXPECT_FALSE(Interval_d::contains(i, 0.0)); - } - - { - const auto i = Interval_d(-1.0, 1.0); - EXPECT_TRUE(Interval_d::contains(i, 0.0)); - EXPECT_FALSE(Interval_d::contains(i, 2.0)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Empty) -{ - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(); - const auto i3 = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i3)); - } - - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(0.0, 1.0); - const auto i3 = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i3)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ZeroMeasure) -{ - { - const auto i = Interval_d(0, 1); - EXPECT_FALSE(Interval_d::zero_measure(i)); - } - - { - const auto i = Interval_d(); - EXPECT_FALSE(Interval_d::zero_measure(i)); - } - - { - const auto i = Interval_d(2, 2); - EXPECT_TRUE(Interval_d::zero_measure(i)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, IntersectionMeasure) -{ - { - const auto i1 = Interval_d(-1.0, 1.0); - const auto i2 = Interval_d(-0.5, 1.5); - const auto i = Interval_d::intersect(i1, i2); - EXPECT_EQ(Interval_d::min(i), -0.5); - EXPECT_EQ(Interval_d::max(i), 1.0); - EXPECT_NEAR(Interval_d::measure(i), 1.5, epsilon); - } - - { - const auto i1 = Interval_d(-2.0, -1.0); - const auto i2 = Interval_d(1.0, 2.0); - const auto i = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i)); - EXPECT_TRUE(std::isnan(Interval_d::min(i))); - EXPECT_TRUE(std::isnan(Interval_d::max(i))); - EXPECT_TRUE(std::isnan(Interval_d::measure(i))); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ConstructionMeasure) -{ - { - const auto i = Interval_d(); - EXPECT_TRUE(std::isnan(Interval_d::min(i))); - EXPECT_TRUE(std::isnan(Interval_d::max(i))); - EXPECT_TRUE(std::isnan(Interval_d::measure(i))); - } - - { - const auto i = Interval_d(-1.0, 1.0); - EXPECT_EQ(Interval_d::min(i), -1.0); - EXPECT_EQ(Interval_d::max(i), 1.0); - EXPECT_NEAR(Interval_d::measure(i), 2.0, epsilon); - } - - { - const auto i = Interval_d(0.0, 0.0); - EXPECT_EQ(Interval_d::min(i), 0.0); - EXPECT_EQ(Interval_d::max(i), 0.0); - EXPECT_FALSE(Interval_d::empty(i)); - EXPECT_EQ(Interval_d::measure(i), 0.0); - } - - { - EXPECT_THROW({ Interval_d(1.0, -1.0); }, std::runtime_error); - } -} - -//------------------------------------------------------------------------------ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp deleted file mode 100644 index 47cb8383fdada..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ - -#if defined(__WIN32) -#if defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ - defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllexport) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#else -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllimport) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#endif -#elif defined(__linux__) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) -#else // defined(_LINUX) -#error "Unsupported Build Configuration" -#endif // defined(_WINDOWS) - -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/plugins_description.xml b/common/autoware_auto_perception_rviz_plugin/plugins_description.xml deleted file mode 100644 index 3f56a43558494..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/plugins_description.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - Convert a PredictedObjects to markers and display them. - - autoware_auto_perception_msgs/msg/PredictedObjects - - - - - Convert a TrackedObjects to markers and display them. - - autoware_auto_perception_msgs/msg/TrackedObjects - - - - - Convert a DetectedObjects to markers and display them. - - autoware_auto_perception_msgs/msg/DetectedObjects - - - diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt deleted file mode 100755 index a8ae9ec2d962e..0000000000000 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_tf2) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) - target_include_directories(test_tf2_autoware_auto_msgs PRIVATE "include") - ament_target_dependencies(test_tf2_autoware_auto_msgs - "autoware_auto_common" - "autoware_auto_perception_msgs" - "autoware_auto_system_msgs" - "autoware_auto_geometry_msgs" - "geometry_msgs" - "tf2" - "tf2_geometry_msgs" - "tf2_ros" - ) -endif() - -ament_auto_package() diff --git a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md b/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md deleted file mode 100644 index aaa719d617373..0000000000000 --- a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md +++ /dev/null @@ -1,183 +0,0 @@ -# autoware_auto_tf2 - -This is the design document for the `autoware_auto_tf2` package. - -## Purpose / Use cases - -In general, users of ROS rely on tf (and its successor, tf2) for publishing and utilizing coordinate -frame transforms. This is true even to the extent that the tf2 contains the packages -`tf2_geometry_msgs` and `tf2_sensor_msgs` which allow for easy conversion to and from the message -types defined in `geometry_msgs` and `sensor_msgs`, respectively. However, AutowareAuto contains -some specialized message types which are not transformable between frames using the ROS 2 library. -The `autoware_auto_tf2` package aims to provide developers with tools to transform applicable -`autoware_auto_msgs` types. In addition to this, this package also provides transform tools for -messages types in `geometry_msgs` missing in `tf2_geometry_msgs`. - -## Design - -While writing `tf2_some_msgs` or contributing to `tf2_geometry_msgs`, compatibility and design -intent was ensured with the following files in the existing tf2 framework: - -- `tf2/convert.h` -- `tf2_ros/buffer_interface.h` - -For example: - -```cpp -void tf2::convert( const A & a,B & b) -``` - -The method `tf2::convert` is dependent on the following: - -```cpp -template - B tf2::toMsg(const A& a); -template - void tf2::fromMsg(const A&, B& b); - -// New way to transform instead of using tf2::doTransform() directly -tf2_ros::BufferInterface::transform(...) -``` - -Which, in turn, is dependent on the following: - -```cpp -void tf2::convert( const A & a,B & b) -const std::string& tf2::getFrameId(const T& t) -const ros::Time& tf2::getTimestamp(const T& t); -``` - -## Current Implementation of tf2_geometry_msgs - -In both ROS 1 and ROS 2 stamped msgs like `Vector3Stamped`, `QuaternionStamped` have associated -functions like: - -- `getTimestamp` -- `getFrameId` -- `doTransform` -- `toMsg` -- `fromMsg` - -In ROS 1, to support `tf2::convert` and need in `doTransform` of the stamped data, non-stamped -underlying data like `Vector3`, `Point`, have implementations of the following functions: - -- `toMsg` -- `fromMsg` - -In ROS 2, much of the `doTransform` method is not using `toMsg` and `fromMsg` as data types from tf2 -are not used. Instead `doTransform` is done using `KDL`, thus functions relating to underlying data -were not added; such as `Vector3`, `Point`, or ported in this commit ros/geometry2/commit/6f2a82. -The non-stamped data with `toMsg` and `fromMsg` are `Quaternion`, `Transform`. `Pose` has the -modified `toMsg` and not used by `PoseStamped`. - -## Plan for autoware_auto_perception_msgs::msg::BoundingBoxArray - -The initial rough plan was to implement some of the common tf2 functions like `toMsg`, `fromMsg`, -and `doTransform`, as needed for all the underlying data types in `BoundingBoxArray`. Examples -of the data types include: `BoundingBox`, `Quaternion32`, and `Point32`. In addition, the -implementation should be done such that upstream contributions could also be made to `geometry_msgs`. - -## Assumptions / Known limits - -Due to conflicts in a function signatures, the predefined template of `convert.h`/ -`transform_functions.h` is not followed and compatibility with `tf2::convert(..)` is broken and -`toMsg` is written differently. - -```cpp -// Old style -geometry_msgs::Vector3 toMsg(const tf2::Vector3& in) -geometry_msgs::Point& toMsg(const tf2::Vector3& in) - -// New style -geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out) -``` - -## Inputs / Outputs / API - - - -The library provides API `doTransform` for the following data-types that are either not available -in `tf2_geometry_msgs` or the messages types are part of `autoware_auto_msgs` and are therefore -custom and not inherently supported by any of the tf2 libraries. The following APIs are provided -for the following data types: - -- `Point32` - -```cpp -inline void doTransform( - const geometry_msgs::msg::Point32 & t_in, - geometry_msgs::msg::Point32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `Quaternion32` (`autoware_auto_msgs`) - -```cpp -inline void doTransform( - const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, - autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `BoundingBox` (`autoware_auto_msgs`) - -```cpp -inline void doTransform( - const BoundingBox & t_in, BoundingBox & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `BoundingBoxArray` - -```cpp -inline void doTransform( - const BoundingBoxArray & t_in, - BoundingBoxArray & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -In addition, the following helper methods are also added: - -- `BoundingBoxArray` - -```cpp -inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t) - -inline std::string getFrameId(const BoundingBoxArray & t) -``` - - - - - - - - - - - - - - -## Future extensions / Unimplemented parts - -## Challenges - -- `tf2_geometry_msgs` does not implement `doTransform` for any non-stamped data types, but it is - possible with the same function template. It is needed when transforming sub-data, with main data - that does have a stamp and can call doTransform on the sub-data with the same transform. Is this a useful upstream contribution? -- `tf2_geometry_msgs` does not have `Point`, `Point32`, does not seem it needs one, also the - implementation of non-standard `toMsg` would not help the convert. -- `BoundingBox` uses 32-bit float like `Quaternion32` and `Point32` to save space, as they are used - repeatedly in `BoundingBoxArray`. While transforming is it better to convert to 64-bit `Quaternion`, - `Point`, or `PoseStamped`, to re-use existing implementation of `doTransform`, or does it need to be - implemented? It may not be simple to template. - - - diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp deleted file mode 100644 index 50c16ba8eaf7d..0000000000000 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ /dev/null @@ -1,147 +0,0 @@ -// Copyright 2020, The Autoware Foundation. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/// \file -/// \brief This file includes common transform functionality for autoware_auto_msgs - -#ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ -#define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include - -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; -using BoundingBoxArray = autoware_auto_perception_msgs::msg::BoundingBoxArray; -using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; - -namespace tf2 -{ -/******************/ -/** Quaternion32 **/ -/******************/ - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs Quaternion32 type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The Quaternion32 message to transform. - * \param t_out The transformed Quaternion32 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, - autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - KDL::Rotation r_in = KDL::Rotation::Quaternion(t_in.x, t_in.y, t_in.z, t_in.w); - KDL::Rotation out = gmTransformToKDL(transform).M * r_in; - - double qx, qy, qz, qw; - out.GetQuaternion(qx, qy, qz, qw); - t_out.x = static_cast(qx); - t_out.y = static_cast(qy); - t_out.z = static_cast(qz); - t_out.w = static_cast(qw); -} - -/******************/ -/** BoundingBox **/ -/******************/ - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBox type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The BoundingBox message to transform. - * \param t_out The transformed BoundingBox message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const BoundingBox & t_in, BoundingBox & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = t_in; - doTransform(t_in.orientation, t_out.orientation, transform); - doTransform(t_in.centroid, t_out.centroid, transform); - doTransform(t_in.corners[0], t_out.corners[0], transform); - doTransform(t_in.corners[1], t_out.corners[1], transform); - doTransform(t_in.corners[2], t_out.corners[2], transform); - doTransform(t_in.corners[3], t_out.corners[3], transform); - // TODO(jitrc): add conversion for other fields of BoundingBox, such as heading, variance, size -} - -/**********************/ -/** BoundingBoxArray **/ -/**********************/ - -/** \brief Extract a timestamp from the header of a BoundingBoxArray message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t A timestamped BoundingBoxArray message to extract the timestamp from. - * \return The timestamp of the message. - */ -template <> -inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t) -{ - return tf2_ros::fromMsg(t.header.stamp); -} - -/** \brief Extract a frame ID from the header of a BoundingBoxArray message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t A timestamped BoundingBoxArray message to extract the frame ID from. - * \return A string containing the frame ID of the message. - */ -template <> -inline std::string getFrameId(const BoundingBoxArray & t) -{ - return t.header.frame_id; -} - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBoxArray type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The BoundingBoxArray to transform, as a timestamped BoundingBoxArray message. - * \param t_out The transformed BoundingBoxArray, as a timestamped BoundingBoxArray message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const BoundingBoxArray & t_in, BoundingBoxArray & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = t_in; - for (auto idx = 0U; idx < t_in.boxes.size(); idx++) { - doTransform(t_out.boxes[idx], t_out.boxes[idx], transform); - } - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; -} - -} // namespace tf2 - -#endif // AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml deleted file mode 100644 index b84e2d77befda..0000000000000 --- a/common/autoware_auto_tf2/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - autoware_auto_tf2 - 1.0.0 - Transform related utilities for different msg types - Jit Ray Chowdhury - Tomoya Kimura - Shumpei Wakabayashi - Satoshi Ota - Apache License 2.0 - - ament_cmake - autoware_cmake - - autoware_auto_common - autoware_auto_geometry_msgs - autoware_auto_perception_msgs - autoware_auto_system_msgs - geometry_msgs - tf2 - tf2_geometry_msgs - tf2_ros - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp deleted file mode 100644 index ce81dd2276870..0000000000000 --- a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp +++ /dev/null @@ -1,318 +0,0 @@ -// Copyright 2020, The Autoware Foundation. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/// \file -/// \brief This file includes common transform functionally for autoware_auto_msgs - -#include -#include - -#include -#include -#include - -#include - -std::unique_ptr tf_buffer = nullptr; -constexpr double EPS = 1e-3; - -geometry_msgs::msg::TransformStamped filled_transform() -{ - geometry_msgs::msg::TransformStamped t; - t.transform.translation.x = 10; - t.transform.translation.y = 20; - t.transform.translation.z = 30; - t.transform.rotation.w = 0; - t.transform.rotation.x = 1; - t.transform.rotation.y = 0; - t.transform.rotation.z = 0; - t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - t.header.frame_id = "A"; - t.child_frame_id = "B"; - - return t; -} - -TEST(Tf2AutowareAuto, DoTransformPoint32) -{ - const auto trans = filled_transform(); - geometry_msgs::msg::Point32 p1; - p1.x = 1; - p1.y = 2; - p1.z = 3; - - // doTransform - geometry_msgs::msg::Point32 p_out; - tf2::doTransform(p1, p_out, trans); - - EXPECT_NEAR(p_out.x, 11, EPS); - EXPECT_NEAR(p_out.y, 18, EPS); - EXPECT_NEAR(p_out.z, 27, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformPolygon) -{ - const auto trans = filled_transform(); - geometry_msgs::msg::Polygon poly; - geometry_msgs::msg::Point32 p1; - p1.x = 1; - p1.y = 2; - p1.z = 3; - poly.points.push_back(p1); - // doTransform - geometry_msgs::msg::Polygon poly_out; - tf2::doTransform(poly, poly_out, trans); - - EXPECT_NEAR(poly_out.points[0].x, 11, EPS); - EXPECT_NEAR(poly_out.points[0].y, 18, EPS); - EXPECT_NEAR(poly_out.points[0].z, 27, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformQuaternion32) -{ - const auto trans = filled_transform(); - autoware_auto_geometry_msgs::msg::Quaternion32 q1; - q1.w = 0; - q1.x = 0; - q1.y = 0; - q1.z = 1; - - // doTransform - autoware_auto_geometry_msgs::msg::Quaternion32 q_out; - tf2::doTransform(q1, q_out, trans); - - EXPECT_NEAR(q_out.x, 0.0, EPS); - EXPECT_NEAR(q_out.y, 1.0, EPS); - EXPECT_NEAR(q_out.z, 0.0, EPS); - EXPECT_NEAR(q_out.w, 0.0, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformBoundingBox) -{ - const auto trans = filled_transform(); - BoundingBox bb1; - bb1.orientation.w = 0; - bb1.orientation.x = 0; - bb1.orientation.y = 0; - bb1.orientation.z = 1; - bb1.centroid.x = 1; - bb1.centroid.y = 2; - bb1.centroid.z = 3; - bb1.corners[0].x = 4; - bb1.corners[0].y = 5; - bb1.corners[0].z = 6; - bb1.corners[1].x = 7; - bb1.corners[1].y = 8; - bb1.corners[1].z = 9; - bb1.corners[2].x = 10; - bb1.corners[2].y = 11; - bb1.corners[2].z = 12; - bb1.corners[3].x = 13; - bb1.corners[3].y = 14; - bb1.corners[3].z = 15; - - // doTransform - BoundingBox bb_out; - tf2::doTransform(bb1, bb_out, trans); - - EXPECT_NEAR(bb_out.orientation.x, 0.0, EPS); - EXPECT_NEAR(bb_out.orientation.y, 1.0, EPS); - EXPECT_NEAR(bb_out.orientation.z, 0.0, EPS); - EXPECT_NEAR(bb_out.orientation.w, 0.0, EPS); - EXPECT_NEAR(bb_out.centroid.x, 11, EPS); - EXPECT_NEAR(bb_out.centroid.y, 18, EPS); - EXPECT_NEAR(bb_out.centroid.z, 27, EPS); - EXPECT_NEAR(bb_out.corners[0].x, 14, EPS); - EXPECT_NEAR(bb_out.corners[0].y, 15, EPS); - EXPECT_NEAR(bb_out.corners[0].z, 24, EPS); - EXPECT_NEAR(bb_out.corners[1].x, 17, EPS); - EXPECT_NEAR(bb_out.corners[1].y, 12, EPS); - EXPECT_NEAR(bb_out.corners[1].z, 21, EPS); - EXPECT_NEAR(bb_out.corners[2].x, 20, EPS); - EXPECT_NEAR(bb_out.corners[2].y, 9, EPS); - EXPECT_NEAR(bb_out.corners[2].z, 18, EPS); - EXPECT_NEAR(bb_out.corners[3].x, 23, EPS); - EXPECT_NEAR(bb_out.corners[3].y, 6, EPS); - EXPECT_NEAR(bb_out.corners[3].z, 15, EPS); - - // Testing unused fields are unmodified - EXPECT_EQ(bb_out.size, bb1.size); - EXPECT_EQ(bb_out.heading, bb1.heading); - EXPECT_EQ(bb_out.heading_rate, bb1.heading_rate); - EXPECT_EQ(bb_out.variance, bb1.variance); - EXPECT_EQ(bb_out.vehicle_label, bb1.vehicle_label); - EXPECT_EQ(bb_out.signal_label, bb1.signal_label); - EXPECT_EQ(bb_out.class_likelihood, bb1.class_likelihood); -} - -TEST(Tf2AutowareAuto, TransformBoundingBoxArray) -{ - BoundingBox bb1; - bb1.orientation.w = 0; - bb1.orientation.x = 0; - bb1.orientation.y = 0; - bb1.orientation.z = 1; - bb1.centroid.x = 20; - bb1.centroid.y = 21; - bb1.centroid.z = 22; - bb1.corners[0].x = 23; - bb1.corners[0].y = 24; - bb1.corners[0].z = 25; - bb1.corners[1].x = 26; - bb1.corners[1].y = 27; - bb1.corners[1].z = 28; - bb1.corners[2].x = 29; - bb1.corners[2].y = 30; - bb1.corners[2].z = 31; - bb1.corners[3].x = 32; - bb1.corners[3].y = 33; - bb1.corners[3].z = 34; - - BoundingBox bb2; - bb2.orientation.w = 0.707f; - bb2.orientation.x = -0.706f; - bb2.orientation.y = 0; - bb2.orientation.z = 0; - bb2.centroid.x = 50; - bb2.centroid.y = 51; - bb2.centroid.z = 52; - bb2.corners[0].x = 53; - bb2.corners[0].y = 54; - bb2.corners[0].z = 55; - bb2.corners[1].x = 56; - bb2.corners[1].y = 57; - bb2.corners[1].z = 58; - bb2.corners[2].x = 59; - bb2.corners[2].y = 50; - bb2.corners[2].z = 51; - bb2.corners[3].x = 52; - bb2.corners[3].y = 53; - bb2.corners[3].z = 54; - - BoundingBoxArray bba1; - bba1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - bba1.header.frame_id = "A"; - bba1.boxes.push_back(bb1); - bba1.boxes.push_back(bb2); - - // simple api - const auto bba_simple = tf_buffer->transform(bba1, "B", tf2::durationFromSec(2.0)); - - EXPECT_EQ(bba_simple.header.frame_id, "B"); - - // checking boxes[0] - EXPECT_NEAR(bba_simple.boxes[0].orientation.x, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.y, 1.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.w, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.x, 10, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.y, -1, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.z, 8, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].x, 13, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].y, -4, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].z, 5, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].x, 16, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].y, -7, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].z, 2, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].x, 19, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].y, -10, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].z, -1, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].x, 22, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].y, -13, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].z, -4, EPS); - - // checking boxes[1] - EXPECT_NEAR(bba_simple.boxes[1].orientation.x, 0.707, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.y, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.w, 0.707, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.x, 40, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.y, -31, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.z, -22, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].x, 43, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].y, -34, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].z, -25, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].x, 46, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].y, -37, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].z, -28, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].x, 49, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].y, -30, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].z, -21, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].x, 42, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].y, -33, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].z, -24, EPS); - - // advanced api - const auto bba_advanced = - tf_buffer->transform(bba1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); - - EXPECT_EQ(bba_advanced.header.frame_id, "B"); - - // checking boxes[0] - EXPECT_NEAR(bba_advanced.boxes[0].orientation.x, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.y, 1.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.w, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.x, 10, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.y, -1, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.z, 8, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].x, 13, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].y, -4, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].z, 5, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].x, 16, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].y, -7, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].z, 2, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].x, 19, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].y, -10, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].z, -1, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].x, 22, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].y, -13, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].z, -4, EPS); - - // checking boxes[1] - EXPECT_NEAR(bba_advanced.boxes[1].orientation.x, 0.707, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.y, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.w, 0.707, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.x, 40, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.y, -31, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.z, -22, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].x, 43, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].y, -34, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].z, -25, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].x, 46, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].y, -37, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].z, -28, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].x, 49, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].y, -30, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].z, -21, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].x, 42, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].y, -33, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].z, -24, EPS); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf_buffer = std::make_unique(clock); - tf_buffer->setUsingDedicatedThread(true); - - // populate buffer - const geometry_msgs::msg::TransformStamped t = filled_transform(); - tf_buffer->setTransform(t, "test"); - - int ret = RUN_ALL_TESTS(); - return ret; -} diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt index aafc62fc90d25..95224b9967d4b 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt @@ -68,7 +68,7 @@ pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) ament_target_dependencies(${PROJECT_NAME} PUBLIC rviz_common rviz_rendering - autoware_auto_vehicle_msgs + autoware_vehicle_msgs tier4_planning_msgs autoware_perception_msgs ) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md index 0d0def1a46997..943d566ad109c 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md @@ -15,13 +15,13 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t | Name | Type | Description | | ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------ | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle velocity | -| `/vehicle/status/turn_indicators_status` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | -| `/vehicle/status/hazard_status` | `autoware_auto_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | -| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic is status of gear | +| `/vehicle/status/velocity_status` | `autoware_vehicle_msgs::msg::VelocityReport` | The topic is vehicle velocity | +| `/vehicle/status/turn_indicators_status` | `autoware_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/hazard_status` | `autoware_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | The topic is status of gear | | `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | The topic is velocity limit | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | The topic is status of traffic light | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | The topic is status of traffic light | ## Parameter diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp index 3fe473d5d5123..cb7e49685d0b3 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_vehicle_msgs/msg/gear_report.hpp" #include #include @@ -37,7 +37,7 @@ class GearDisplay public: GearDisplay(); void drawGearIndicator(QPainter & painter, const QRectF & backgroundRect); - void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); private: int current_gear_; // Internal variable to store current gear diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp index ec1acb9a5dc68..0831ded468c99 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -95,29 +95,29 @@ private Q_SLOTS: std::unique_ptr traffic_display_; std::unique_ptr speed_limit_display_; - rclcpp::Subscription::SharedPtr gear_sub_; - rclcpp::Subscription::SharedPtr steering_sub_; - rclcpp::Subscription::SharedPtr speed_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr gear_sub_; + rclcpp::Subscription::SharedPtr steering_sub_; + rclcpp::Subscription::SharedPtr speed_sub_; + rclcpp::Subscription::SharedPtr turn_signals_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr hazard_lights_sub_; - rclcpp::Subscription::SharedPtr traffic_sub_; + rclcpp::Subscription::SharedPtr + traffic_sub_; rclcpp::Subscription::SharedPtr speed_limit_sub_; std::mutex property_mutex_; - void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); - void updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); void updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); void updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); void drawWidget(QImage & hud); }; } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp index 0669028dba3b2..2ae8e9a3fe0b9 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -37,7 +37,7 @@ class SpeedDisplay public: SpeedDisplay(); void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: float current_speed_; // Internal variable to store current speed diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp index b59f4acf380db..fcdb293fe8c72 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -39,7 +39,7 @@ class SpeedLimitDisplay SpeedLimitDisplay(); void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: float current_limit; // Internal variable to store current gear diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp index 121ee9a0a4d84..dada3cddab911 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include #include @@ -37,8 +37,7 @@ class SteeringWheelDisplay public: SteeringWheelDisplay(); void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect); - void updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); private: float steering_angle_ = 0.0f; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp index efa30f37ccb3e..fd15f542021f1 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp @@ -23,8 +23,9 @@ #include #include -#include -#include +#include +#include +#include #include #include @@ -39,8 +40,8 @@ class TrafficDisplay TrafficDisplay(); void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg); - autoware_perception_msgs::msg::TrafficSignal current_traffic_; + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficLightGroupArray current_traffic_; private: QImage traffic_light_image_; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp index ca10c92c06a3e..022de6d8c22c9 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp @@ -23,8 +23,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -41,9 +41,9 @@ class TurnSignalsDisplay TurnSignalsDisplay(); void drawArrows(QPainter & painter, const QRectF & backgroundRect, const QColor & color); void updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); void updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); private: QImage arrowImage; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml index 4d2f53e1e27ed..73e0dbea0866e 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml @@ -11,8 +11,8 @@ BSD-3-Clause - autoware_auto_vehicle_msgs autoware_perception_msgs + autoware_vehicle_msgs boost rviz_common rviz_ogre_vendor diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp index 825c7c1620e2c..956172bef6ed6 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp @@ -49,8 +49,7 @@ GearDisplay::GearDisplay() : current_gear_(0) } } -void GearDisplay::updateGearData( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +void GearDisplay::updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) { current_gear_ = msg->report; // Assuming msg->report contains the gear information } @@ -60,19 +59,19 @@ void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroun // we deal with the different gears here std::string gearString; switch (current_gear_) { - case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: + case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: gearString = "N"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::LOW: - case autoware_auto_vehicle_msgs::msg::GearReport::LOW_2: + case autoware_vehicle_msgs::msg::GearReport::LOW: + case autoware_vehicle_msgs::msg::GearReport::LOW_2: gearString = "L"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::NONE: - case autoware_auto_vehicle_msgs::msg::GearReport::PARK: + case autoware_vehicle_msgs::msg::GearReport::NONE: + case autoware_vehicle_msgs::msg::GearReport::PARK: gearString = "P"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE_2: + case autoware_vehicle_msgs::msg::GearReport::REVERSE: + case autoware_vehicle_msgs::msg::GearReport::REVERSE_2: gearString = "R"; break; // all the drive gears from DRIVE to DRIVE_16 diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp index 9add6336ece46..ccfdaa69f5d2a 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -76,31 +76,29 @@ void SignalDisplay::onInitialize() auto rviz_ros_node = context_->getRosNodeAbstraction(); gear_topic_property_ = std::make_unique( - "Gear Topic Test", "/vehicle/status/gear_status", "autoware_auto_vehicle_msgs/msg/GearReport", + "Gear Topic Test", "/vehicle/status/gear_status", "autoware_vehicle_msgs/msg/GearReport", "Topic for Gear Data", this, SLOT(topic_updated_gear())); gear_topic_property_->initialize(rviz_ros_node); turn_signals_topic_property_ = std::make_unique( "Turn Signals Topic", "/vehicle/status/turn_indicators_status", - "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, + "autoware_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, SLOT(topic_updated_turn_signals())); turn_signals_topic_property_->initialize(rviz_ros_node); speed_topic_property_ = std::make_unique( - "Speed Topic", "/vehicle/status/velocity_status", - "autoware_auto_vehicle_msgs/msg/VelocityReport", "Topic for Speed Data", this, - SLOT(topic_updated_speed())); + "Speed Topic", "/vehicle/status/velocity_status", "autoware_vehicle_msgs/msg/VelocityReport", + "Topic for Speed Data", this, SLOT(topic_updated_speed())); speed_topic_property_->initialize(rviz_ros_node); steering_topic_property_ = std::make_unique( - "Steering Topic", "/vehicle/status/steering_status", - "autoware_auto_vehicle_msgs/msg/SteeringReport", "Topic for Steering Data", this, - SLOT(topic_updated_steering())); + "Steering Topic", "/vehicle/status/steering_status", "autoware_vehicle_msgs/msg/SteeringReport", + "Topic for Steering Data", this, SLOT(topic_updated_steering())); steering_topic_property_->initialize(rviz_ros_node); hazard_lights_topic_property_ = std::make_unique( "Hazard Lights Topic", "/vehicle/status/hazard_lights_status", - "autoware_auto_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, + "autoware_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, SLOT(topic_updated_hazard_lights())); hazard_lights_topic_property_->initialize(rviz_ros_node); @@ -111,10 +109,9 @@ void SignalDisplay::onInitialize() speed_limit_topic_property_->initialize(rviz_ros_node); traffic_topic_property_ = std::make_unique( - "Traffic Topic", - "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal", - "autoware_perception/msgs/msg/TrafficSignal", "Topic for Traffic Light Data", this, - SLOT(topic_updated_traffic())); + "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals", + "autoware_perception_msgs/msgs/msg/TrafficLightGroupArray", "Topic for Traffic Light Data", + this, SLOT(topic_updated_traffic())); traffic_topic_property_->initialize(rviz_ros_node); } @@ -192,7 +189,7 @@ void SignalDisplay::onDisable() } void SignalDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(property_mutex_); @@ -213,7 +210,7 @@ void SignalDisplay::updateSpeedLimitData( } void SignalDisplay::updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -224,7 +221,7 @@ void SignalDisplay::updateHazardLightsData( } void SignalDisplay::updateGearData( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -235,7 +232,7 @@ void SignalDisplay::updateGearData( } void SignalDisplay::updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -246,7 +243,7 @@ void SignalDisplay::updateSteeringData( } void SignalDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -258,7 +255,7 @@ void SignalDisplay::updateSpeedData( } void SignalDisplay::updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -372,11 +369,10 @@ void SignalDisplay::topic_updated_gear() gear_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); gear_sub_ = - rviz_ros_node->get_raw_node()->create_subscription( - gear_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { - updateGearData(msg); - }); + rviz_ros_node->get_raw_node()->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::GearReport::SharedPtr msg) { updateGearData(msg); }); } void SignalDisplay::topic_updated_steering() @@ -384,12 +380,13 @@ void SignalDisplay::topic_updated_steering() // resubscribe to the topic steering_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - steering_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - steering_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { - updateSteeringData(msg); - }); + steering_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); } void SignalDisplay::topic_updated_speed() @@ -397,12 +394,13 @@ void SignalDisplay::topic_updated_speed() // resubscribe to the topic speed_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - speed_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - speed_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { - updateSpeedData(msg); - }); + speed_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); } void SignalDisplay::topic_updated_speed_limit() @@ -427,9 +425,10 @@ void SignalDisplay::topic_updated_turn_signals() turn_signals_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - turn_signals_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + ->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { updateTurnSignalsData(msg); }); } @@ -442,9 +441,10 @@ void SignalDisplay::topic_updated_hazard_lights() hazard_lights_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - hazard_lights_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + ->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { updateHazardLightsData(msg); }); } @@ -454,12 +454,14 @@ void SignalDisplay::topic_updated_traffic() // resubscribe to the topic traffic_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - traffic_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - traffic_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) { - updateTrafficLightData(msg); - }); + traffic_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficLightGroupArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); } } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp index 5c5342259005b..b9c22ec5f53ac 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -50,7 +50,7 @@ SpeedDisplay::SpeedDisplay() : current_speed_(0.0) } void SpeedDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { try { // Assuming msg->state.longitudinal_velocity_mps is the field you're interested in @@ -64,7 +64,7 @@ void SpeedDisplay::updateSpeedData( } // void SpeedDisplay::processMessage(const -// autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) +// autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) // { // try // { diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp index 4c83b4a73c0c2..3dec6a8e7d4a0 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp @@ -57,7 +57,7 @@ void SpeedLimitDisplay::updateSpeedLimitData( } void SpeedLimitDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { try { float speed = msg->longitudinal_velocity; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp index 45ebcde086165..d2390b16e5373 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp @@ -56,7 +56,7 @@ SteeringWheelDisplay::SteeringWheelDisplay() } void SteeringWheelDisplay::updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) { try { // Assuming msg->steering_angle is the field you're interested in diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp index f6d232709a333..2dc9c23583a52 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp @@ -48,7 +48,7 @@ TrafficDisplay::TrafficDisplay() } void TrafficDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg) { current_traffic_ = *msg; } @@ -68,8 +68,8 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF backgroundRect.top() + circleRect.height() / 2)); painter.drawEllipse(circleRect); - if (!current_traffic_.elements.empty()) { - switch (current_traffic_.elements[0].color) { + if (!current_traffic_.traffic_light_groups.empty()) { + switch (current_traffic_.traffic_light_groups[0].elements[0].color) { case 1: painter.setBrush(QBrush(tl_red_)); painter.drawEllipse(circleRect); diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp index 1aaa5871015a8..b42b5d953fcc6 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp @@ -46,7 +46,7 @@ TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0) } void TurnSignalsDisplay::updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) { try { // Assuming msg->report is the field you're interested in @@ -58,7 +58,7 @@ void TurnSignalsDisplay::updateTurnSignalsData( } void TurnSignalsDisplay::updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) { try { // Assuming msg->report is the field you're interested in @@ -80,11 +80,11 @@ void TurnSignalsDisplay::drawArrows( int rightArrowXPos = backgroundRect.right() - scaledRightArrow.width() * 3 - 175; bool leftActive = - (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || - current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + (current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || + current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE); bool rightActive = - (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || - current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + (current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || + current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE); // Color the arrows based on the state of the turn signals and hazard lights by having them blink // on and off diff --git a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt b/common/autoware_perception_rviz_plugin/CMakeLists.txt similarity index 68% rename from common/autoware_auto_perception_rviz_plugin/CMakeLists.txt rename to common/autoware_perception_rviz_plugin/CMakeLists.txt index 8d0469e78c3ac..74671d74f7397 100644 --- a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt +++ b/common/autoware_perception_rviz_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(autoware_auto_perception_rviz_plugin) +project(autoware_perception_rviz_plugin) find_package(autoware_cmake REQUIRED) autoware_package() @@ -13,18 +13,18 @@ set(OD_PLUGIN_LIB_SRC ) set(OD_PLUGIN_LIB_HEADERS - include/autoware_auto_perception_rviz_plugin/visibility_control.hpp + include/autoware_perception_rviz_plugin/visibility_control.hpp ) set(OD_PLUGIN_LIB_HEADERS_TO_WRAP - include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp + include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp + include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp + include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp ) set(COMMON_HEADERS - include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp + include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp + include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp + include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp ) set(COMMON_SRC diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_perception_rviz_plugin/README.md similarity index 63% rename from common/autoware_auto_perception_rviz_plugin/README.md rename to common/autoware_perception_rviz_plugin/README.md index cb5deb48f411c..ed6f3e1675ace 100644 --- a/common/autoware_auto_perception_rviz_plugin/README.md +++ b/common/autoware_perception_rviz_plugin/README.md @@ -1,4 +1,4 @@ -# autoware_auto_perception_plugin +# autoware_perception_rviz_plugin ## Purpose @@ -19,9 +19,9 @@ Example: #### Input Types -| Name | Type | Description | -| ---- | ----------------------------------------------------- | ---------------------- | -| | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array | +| Name | Type | Description | +| ---- | ------------------------------------------------ | ---------------------- | +| | `autoware_perception_msgs::msg::DetectedObjects` | detection result array | #### Visualization Result @@ -31,9 +31,9 @@ Example: #### Input Types -| Name | Type | Description | -| ---- | ---------------------------------------------------- | --------------------- | -| | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array | +| Name | Type | Description | +| ---- | ----------------------------------------------- | --------------------- | +| | `autoware_perception_msgs::msg::TrackedObjects` | tracking result array | #### Visualization Result @@ -45,9 +45,9 @@ Overwrite tracking results with detection results. #### Input Types -| Name | Type | Description | -| ---- | ------------------------------------------------------ | ----------------------- | -| | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | +| Name | Type | Description | +| ---- | ------------------------------------------------- | ----------------------- | +| | `autoware_perception_msgs::msg::PredictedObjects` | prediction result array | #### Visualization Result diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/DetectedObjects.png b/common/autoware_perception_rviz_plugin/icons/classes/DetectedObjects.png similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/icons/classes/DetectedObjects.png rename to common/autoware_perception_rviz_plugin/icons/classes/DetectedObjects.png diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/PredictedObjects.png b/common/autoware_perception_rviz_plugin/icons/classes/PredictedObjects.png similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/icons/classes/PredictedObjects.png rename to common/autoware_perception_rviz_plugin/icons/classes/PredictedObjects.png diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/TrackedObjects.png b/common/autoware_perception_rviz_plugin/icons/classes/TrackedObjects.png similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/icons/classes/TrackedObjects.png rename to common/autoware_perception_rviz_plugin/icons/classes/TrackedObjects.png diff --git a/common/autoware_auto_perception_rviz_plugin/images/detected-object-visualization-description.jpg b/common/autoware_perception_rviz_plugin/images/detected-object-visualization-description.jpg similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/images/detected-object-visualization-description.jpg rename to common/autoware_perception_rviz_plugin/images/detected-object-visualization-description.jpg diff --git a/common/autoware_auto_perception_rviz_plugin/images/predicted-object-visualization-description.jpg b/common/autoware_perception_rviz_plugin/images/predicted-object-visualization-description.jpg similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/images/predicted-object-visualization-description.jpg rename to common/autoware_perception_rviz_plugin/images/predicted-object-visualization-description.jpg diff --git a/common/autoware_auto_perception_rviz_plugin/images/tracked-object-visualization-description.jpg b/common/autoware_perception_rviz_plugin/images/tracked-object-visualization-description.jpg similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/images/tracked-object-visualization-description.jpg rename to common/autoware_perception_rviz_plugin/images/tracked-object-visualization-description.jpg diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp similarity index 81% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp index 10dc46e55ec70..e39faeb254add 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp @@ -11,10 +11,10 @@ // 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_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ -#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" +#include "autoware_perception_rviz_plugin/visibility_control.hpp" #include #include @@ -31,7 +31,7 @@ namespace rviz_plugins namespace common { /// \brief Class to define Color and Alpha values as plugin properties -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty { public: /// \brief Constructor @@ -56,4 +56,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp similarity index 58% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp index 97479fb68ca9b..4dcc4e9ea1545 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp @@ -11,12 +11,12 @@ // 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_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include namespace autoware { @@ -25,13 +25,13 @@ namespace rviz_plugins namespace object_detection { /// \brief Class defining rviz plugin to visualize DetectedObjects -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay -: public ObjectPolygonDisplayBase +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; + using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; DetectedObjectsDisplay(); @@ -43,4 +43,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp similarity index 61% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index 4f545d194b2c2..8d29900e1da26 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. /// \brief This file defines some helper functions used by ObjectPolygonDisplayBase class -#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ -#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" +#include "autoware_perception_rviz_plugin/visibility_control.hpp" #include #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -62,21 +62,19 @@ enum class ObjectFillType { Skeleton, Fill }; // Map defining colors according to value of label field in ObjectClassification msg const std::map< - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> + autoware_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> // Color map is based on cityscapes color kDefaultObjectPropertyValues = { - {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, - {"UNKNOWN", {255, 255, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + {autoware_perception_msgs::msg::ObjectClassification::UNKNOWN, {"UNKNOWN", {255, 255, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN, {"PEDESTRIAN", {255, 192, 203}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + {autoware_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, + {autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE, {"MOTORCYCLE", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, - {"TRAILER", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; + {autoware_perception_msgs::msg::ObjectClassification::TRAILER, {"TRAILER", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param shape_msg Shape msg to be converted. Corners should be in object-local frame @@ -85,17 +83,17 @@ const std::map< /// \param color_rgba Color and alpha values to use for the marker /// \param line_width Line thickness around the object /// \return Marker ptr. Id and header will have to be set by the caller -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available = true, const ObjectFillType fill_type = ObjectFillType::Skeleton); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available = true); @@ -103,137 +101,137 @@ get_2d_shape_marker_ptr( /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param centroid Centroid position of the shape in Object.header.frame_id frame /// \return Marker ptr. Id and header will have to be set by the caller -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_uuid_marker_ptr( const std::string & uuid, const geometry_msgs::msg::Point & centroid, const std_msgs::msg::ColorRGBA & color_rgba); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & confidence_interval_coefficient); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, const double & confidence_interval_coefficient, const double & line_width); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, const std_msgs::msg::ColorRGBA & color_rgba); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, const std_msgs::msg::ColorRGBA & color_rgba); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & confidence_interval_coefficient); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & confidence_interval_coefficient, const double & line_width); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( const double start_angle, const double end_angle, const double radius, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( const double point_list[][3], const int point_pairs[][2], const int & num_pairs, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list( + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list( + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( const geometry_msgs::msg::Point center, const double radius, std::vector & points, const int n); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( - const autoware_auto_perception_msgs::msg::PredictedPath & paths, +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( + const autoware_perception_msgs::msg::PredictedPath & paths, std::vector & points, const bool is_simple = false); /// \brief Convert Point32 to Point /// \param val Point32 to be converted /// \return Point type -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_point( +inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_point( const geometry_msgs::msg::Point32 & val) { geometry_msgs::msg::Point ret; @@ -247,7 +245,7 @@ inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_ /// \param point /// \param orientation /// \return Pose type -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_pose( +inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_pose( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Quaternion & orientation) { geometry_msgs::msg::Pose ret; @@ -256,7 +254,7 @@ inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_p return ret; } -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose() +inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose() { geometry_msgs::msg::Pose pose; pose.position.x = 0.0; @@ -275,8 +273,8 @@ inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose init /// \param logger_name Name to use for logger in case of a warning (if labels is empty) /// \return Id of the best classification, Unknown if there is no best label template -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC + autoware_perception_msgs::msg::ObjectClassification::_label_type get_best_label(ClassificationContainerT labels, const std::string & logger_name) { const auto best_class_label = std::max_element( @@ -287,7 +285,7 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC rclcpp::get_logger(logger_name), "Empty classification field. " "Treating as unknown"); - return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + return autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; } return best_class_label->label; } @@ -297,4 +295,4 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp similarity index 95% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 1093f6e4f2414..b05ba5f27f551 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -11,12 +11,12 @@ // 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_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" -#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" +#include "autoware_perception_rviz_plugin/common/color_alpha_property.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" +#include "autoware_perception_rviz_plugin/visibility_control.hpp" #include #include @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include @@ -46,14 +46,14 @@ namespace object_detection /// classes. /// \tparam MsgT PredictedObjects or TrackedObjects or DetectedObjects type template -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase : public rviz_common::RosTopicDisplay { public: using Color = std::array; using Marker = visualization_msgs::msg::Marker; using MarkerCommon = rviz_default_plugins::displays::MarkerCommon; - using ObjectClassificationMsg = autoware_auto_perception_msgs::msg::ObjectClassification; + using ObjectClassificationMsg = autoware_perception_msgs::msg::ObjectClassification; using RosTopicDisplay = rviz_common::RosTopicDisplay; using PolygonPropertyMap = @@ -189,7 +189,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \return Marker ptr. Id and header will have to be set by the caller template std::optional get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const ClassificationContainerT & labels, const double & line_width, const bool & is_orientation_available) const @@ -212,7 +212,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase template visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available); @@ -363,8 +363,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_predicted_path_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path) const { if (m_display_predicted_paths_property.getBool()) { const std::string uuid_str = uuid_to_string(uuid); @@ -379,7 +379,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_path_confidence_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + const autoware_perception_msgs::msg::PredictedPath & predicted_path) const { if (m_display_path_confidence_property.getBool()) { const std::string uuid_str = uuid_to_string(uuid); @@ -431,7 +431,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase return ss.str(); } - std_msgs::msg::ColorRGBA AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC + std_msgs::msg::ColorRGBA AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC get_color_from_uuid(const std::string & uuid) const { int i = (static_cast(uuid.at(0)) * 4 + static_cast(uuid.at(1))) % @@ -576,4 +576,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp similarity index 85% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp index 775c18db6ba5c..1445f02e34290 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp @@ -11,12 +11,12 @@ // 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_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include #include #include @@ -38,13 +38,13 @@ namespace rviz_plugins namespace object_detection { /// \brief Class defining rviz plugin to visualize PredictedObjects -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay -: public ObjectPolygonDisplayBase +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; + using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; PredictedObjectsDisplay(); ~PredictedObjectsDisplay() @@ -153,4 +153,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp similarity index 80% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp index 4e86a5ee93fd8..9ccaa5cd150f6 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp @@ -11,12 +11,12 @@ // 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_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include #include #include @@ -33,14 +33,14 @@ namespace rviz_plugins namespace object_detection { /// \brief Class defining rviz plugin to visualize TrackedObjects -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay -: public ObjectPolygonDisplayBase +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using TrackedObject = autoware_auto_perception_msgs::msg::TrackedObject; - using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; + using TrackedObject = autoware_perception_msgs::msg::TrackedObject; + using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects; TrackedObjectsDisplay(); @@ -114,4 +114,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/visibility_control.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/visibility_control.hpp new file mode 100644 index 0000000000000..e5e7693054ec8 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/visibility_control.hpp @@ -0,0 +1,43 @@ +// Copyright 2019 the Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) +#if defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ + defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllexport) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#else +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllimport) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#endif +#elif defined(__linux__) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) +#else // defined(_LINUX) +#error "Unsupported Build Configuration" +#endif // defined(_WINDOWS) + +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_perception_rviz_plugin/package.xml similarity index 92% rename from common/autoware_auto_perception_rviz_plugin/package.xml rename to common/autoware_perception_rviz_plugin/package.xml index 2033239824d95..460186c33b7d8 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_perception_rviz_plugin/package.xml @@ -1,7 +1,7 @@ - autoware_auto_perception_rviz_plugin + autoware_perception_rviz_plugin 1.0.0 Contains plugins to visualize object detection outputs Apex.AI, Inc. @@ -18,7 +18,7 @@ libboost-dev qtbase5-dev - autoware_auto_perception_msgs + autoware_perception_msgs rviz_common rviz_default_plugins diff --git a/common/autoware_perception_rviz_plugin/plugins_description.xml b/common/autoware_perception_rviz_plugin/plugins_description.xml new file mode 100644 index 0000000000000..710e3aa2bfa26 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/plugins_description.xml @@ -0,0 +1,26 @@ + + + + + + + Convert a PredictedObjects to markers and display them. + + autoware_perception_msgs/msg/PredictedObjects + + + + + Convert a TrackedObjects to markers and display them. + + autoware_perception_msgs/msg/TrackedObjects + + + + + Convert a DetectedObjects to markers and display them. + + autoware_perception_msgs/msg/DetectedObjects + + + diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_perception_rviz_plugin/src/common/color_alpha_property.cpp similarity index 95% rename from common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp rename to common/autoware_perception_rviz_plugin/src/common/color_alpha_property.cpp index b3e542a02243b..9b8ac2e38b740 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp +++ b/common/autoware_perception_rviz_plugin/src/common/color_alpha_property.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" +#include "autoware_perception_rviz_plugin/common/color_alpha_property.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp similarity index 97% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 53e935fa1850a..9dd0b2923f09f 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp" +#include "autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp" #include @@ -40,7 +40,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) object.kinematics.pose_with_covariance.pose.orientation, object.classification, get_line_width(), object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp similarity index 96% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 7631acffafdf9..1d06454582a2f 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License.. -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" #include #include @@ -50,7 +50,7 @@ namespace detail using Marker = visualization_msgs::msg::Marker; visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color) { auto marker_ptr = std::make_shared(); @@ -70,8 +70,8 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple) { auto marker_ptr = std::make_shared(); @@ -492,7 +492,7 @@ visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available, const ObjectFillType fill_type) @@ -502,7 +502,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( marker_ptr->color = color_rgba; marker_ptr->scale.x = line_width; - using autoware_auto_perception_msgs::msg::Shape; + using autoware_perception_msgs::msg::Shape; if (shape_msg.type == Shape::BOUNDING_BOX) { if (fill_type == ObjectFillType::Skeleton) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; @@ -542,7 +542,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available) @@ -550,7 +550,7 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); - using autoware_auto_perception_msgs::msg::Shape; + using autoware_perception_msgs::msg::Shape; if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); @@ -597,7 +597,7 @@ void calc_line_list_from_points( } void calc_bounding_box_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -620,7 +620,7 @@ void calc_bounding_box_line_list( } void calc_bounding_box_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { // direction triangle @@ -646,7 +646,7 @@ void calc_bounding_box_direction_line_list( } void calc_bounding_box_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -671,7 +671,7 @@ void calc_bounding_box_orientation_line_list( } void calc_2d_bounding_box_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -697,7 +697,7 @@ void calc_2d_bounding_box_bottom_line_list( } void calc_2d_bounding_box_bottom_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -721,7 +721,7 @@ void calc_2d_bounding_box_bottom_direction_line_list( } void calc_2d_bounding_box_bottom_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -746,7 +746,7 @@ void calc_2d_bounding_box_bottom_orientation_line_list( } void calc_cylinder_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double radius = shape.dimensions.x * 0.5; @@ -789,7 +789,7 @@ void calc_cylinder_line_list( } void calc_2d_cylinder_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double radius = shape.dimensions.x * 0.5; @@ -837,7 +837,7 @@ void calc_circle_line_list( } void calc_polygon_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { if (shape.footprint.points.size() < 2) { @@ -890,7 +890,7 @@ void calc_polygon_line_list( } void calc_2d_polygon_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { if (shape.footprint.points.size() < 2) { @@ -917,7 +917,7 @@ void calc_2d_polygon_bottom_line_list( } void calc_path_line_list( - const autoware_auto_perception_msgs::msg::PredictedPath & paths, + const autoware_perception_msgs::msg::PredictedPath & paths, std::vector & points, const bool is_simple) { const int circle_line_num = is_simple ? 5 : 10; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp similarity index 98% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index b42ffe1804246..d11aba912854d 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp" +#include "autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp" #include #include @@ -295,7 +295,7 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) lock.unlock(); - ObjectPolygonDisplayBase::update( + ObjectPolygonDisplayBase::update( wall_dt, ros_dt); } diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp similarity index 98% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 504b51f850444..214ed9ce70e63 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp" +#include "autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp" #include @@ -61,7 +61,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) object.kinematics.pose_with_covariance.pose.orientation, object.classification, get_line_width(), object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; diff --git a/common/autoware_point_types/README.md b/common/autoware_point_types/README.md new file mode 100644 index 0000000000000..92f19d2bc353a --- /dev/null +++ b/common/autoware_point_types/README.md @@ -0,0 +1 @@ +# Autoware Point Types diff --git a/common/component_interface_specs/include/component_interface_specs/localization.hpp b/common/component_interface_specs/include/component_interface_specs/localization.hpp index 70c590c837927..e00d8cef1840d 100644 --- a/common/component_interface_specs/include/component_interface_specs/localization.hpp +++ b/common/component_interface_specs/include/component_interface_specs/localization.hpp @@ -18,16 +18,16 @@ #include #include -#include #include #include +#include namespace localization_interface { struct Initialize { - using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization; + using Service = tier4_localization_msgs::srv::InitializeLocalization; static constexpr char name[] = "/localization/initialize"; }; diff --git a/common/component_interface_specs/include/component_interface_specs/perception.hpp b/common/component_interface_specs/include/component_interface_specs/perception.hpp index 8665b9c35157d..0c72dbdb12078 100644 --- a/common/component_interface_specs/include/component_interface_specs/perception.hpp +++ b/common/component_interface_specs/include/component_interface_specs/perception.hpp @@ -17,14 +17,14 @@ #include -#include +#include namespace perception_interface { struct ObjectRecognition { - using Message = autoware_auto_perception_msgs::msg::PredictedObjects; + using Message = autoware_perception_msgs::msg::PredictedObjects; static constexpr char name[] = "/perception/object_recognition/objects"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index 9efd8c871f68f..58aba53d8ac8a 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include #include @@ -66,7 +66,7 @@ struct LaneletRoute struct Trajectory { - using Message = autoware_auto_planning_msgs::msg::Trajectory; + using Message = autoware_planning_msgs::msg::Trajectory; static constexpr char name[] = "/planning/scenario_planning/trajectory"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/vehicle.hpp b/common/component_interface_specs/include/component_interface_specs/vehicle.hpp index 6358b35c3c674..e7ce2c5212a25 100644 --- a/common/component_interface_specs/include/component_interface_specs/vehicle.hpp +++ b/common/component_interface_specs/include/component_interface_specs/vehicle.hpp @@ -20,10 +20,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include namespace vehicle_interface @@ -31,7 +31,7 @@ namespace vehicle_interface struct SteeringStatus { - using Message = autoware_auto_vehicle_msgs::msg::SteeringReport; + using Message = autoware_vehicle_msgs::msg::SteeringReport; static constexpr char name[] = "/vehicle/status/steering_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -40,7 +40,7 @@ struct SteeringStatus struct GearStatus { - using Message = autoware_auto_vehicle_msgs::msg::GearReport; + using Message = autoware_vehicle_msgs::msg::GearReport; static constexpr char name[] = "/vehicle/status/gear_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -49,7 +49,7 @@ struct GearStatus struct TurnIndicatorStatus { - using Message = autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; + using Message = autoware_vehicle_msgs::msg::TurnIndicatorsReport; static constexpr char name[] = "/vehicle/status/turn_indicators_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -58,7 +58,7 @@ struct TurnIndicatorStatus struct HazardLightStatus { - using Message = autoware_auto_vehicle_msgs::msg::HazardLightsReport; + using Message = autoware_vehicle_msgs::msg::HazardLightsReport; static constexpr char name[] = "/vehicle/status/hazard_lights_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 1ad5f410a814a..fa57bb1641eed 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -12,15 +12,15 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_msgs nav_msgs rcl rclcpp rosidl_runtime_cpp tier4_control_msgs + tier4_localization_msgs tier4_map_msgs tier4_planning_msgs tier4_system_msgs diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index e065f332b75e4..163bd761407c5 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -17,7 +17,6 @@ /// \copyright Copyright 2021 Apex.AI, Inc. /// All rights reserved. -#include #include #include @@ -29,7 +28,7 @@ #include #include -using autoware::common::types::bool8_t; +using bool8_t = bool; using FakeNodeFixture = autoware::tools::testing::FakeTestNode; using FakeNodeFixtureParametrized = autoware::tools::testing::FakeTestNodeParametrized; diff --git a/common/goal_distance_calculator/Readme.md b/common/goal_distance_calculator/Readme.md index 3a25e147cf8e6..062b23baabe47 100644 --- a/common/goal_distance_calculator/Readme.md +++ b/common/goal_distance_calculator/Readme.md @@ -10,10 +10,10 @@ This node publishes deviation of self-pose from goal pose. ### Input -| Name | Type | Description | -| ---------------------------------- | ----------------------------------------- | --------------------- | -| `/planning/mission_planning/route` | `autoware_auto_planning_msgs::msg::Route` | Used to get goal pose | -| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | +| Name | Type | Description | +| ---------------------------------- | ------------------------------------ | --------------------- | +| `/planning/mission_planning/route` | `autoware_planning_msgs::msg::Route` | Used to get goal pose | +| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | ### Output diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp index b4a98d90c74e4..3329c1349b707 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ struct Param struct Input { geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose; - autoware_auto_planning_msgs::msg::Route::ConstSharedPtr route; + autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route; }; struct Output diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp index 50051d928b6c1..680c4a3cdfff1 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -46,14 +46,14 @@ class GoalDistanceCalculatorNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_initial_pose_; tier4_autoware_utils::SelfPoseListener self_pose_listener_; - rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Subscription::SharedPtr sub_route_; // Data Buffer geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - autoware_auto_planning_msgs::msg::Route::SharedPtr route_; + autoware_planning_msgs::msg::LaneletRoute::SharedPtr route_; // Callback - void onRoute(const autoware_auto_planning_msgs::msg::Route::ConstSharedPtr & msg); + void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & msg); // Publisher tier4_autoware_utils::DebugPublisher debug_publisher_; diff --git a/common/goal_distance_calculator/package.xml b/common/goal_distance_calculator/package.xml index 49cb674f0a256..eb71dc45f31a3 100644 --- a/common/goal_distance_calculator/package.xml +++ b/common/goal_distance_calculator/package.xml @@ -10,7 +10,7 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs geometry_msgs rclcpp rclcpp_components diff --git a/common/goal_distance_calculator/src/goal_distance_calculator.cpp b/common/goal_distance_calculator/src/goal_distance_calculator.cpp index 1405d5bd62f7d..719baef283791 100755 --- a/common/goal_distance_calculator/src/goal_distance_calculator.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator.cpp @@ -21,7 +21,7 @@ Output GoalDistanceCalculator::update(const Input & input) Output output{}; output.goal_deviation = - tier4_autoware_utils::calcPoseDeviation(input.route->goal_point.pose, input.current_pose->pose); + tier4_autoware_utils::calcPoseDeviation(input.route->goal_pose, input.current_pose->pose); return output; } diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp index c6062d2a156ee..ab1c35e246719 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -48,9 +48,9 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions goal_distance_calculator_->setParam(param_); // Subscriber - sub_route_ = create_subscription( + sub_route_ = create_subscription( "/planning/mission_planning/route", queue_size, - [&](const autoware_auto_planning_msgs::msg::Route::SharedPtr msg_ptr) { route_ = msg_ptr; }); + [&](const autoware_planning_msgs::msg::LaneletRoute::SharedPtr msg_ptr) { route_ = msg_ptr; }); // Wait for first self pose self_pose_listener_.waitForFirstPose(); diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp index a7d7012c19e22..09973826387a2 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -199,7 +199,7 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::createPoint; std::vector points; diff --git a/common/motion_utils/README.md b/common/motion_utils/README.md index 925ee5b162db3..fd18540a9d0cf 100644 --- a/common/motion_utils/README.md +++ b/common/motion_utils/README.md @@ -47,7 +47,7 @@ The second function finds the nearest index in the lane whose id is `lane_id`. ```cpp size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id); ``` @@ -112,6 +112,6 @@ const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_ ## For developers -Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. +Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. `motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/motion_utils/docs/vehicle/vehicle.md b/common/motion_utils/docs/vehicle/vehicle.md index 9d33b5ed372c4..c768e6115731c 100644 --- a/common/motion_utils/docs/vehicle/vehicle.md +++ b/common/motion_utils/docs/vehicle/vehicle.md @@ -83,10 +83,10 @@ This class check whether the vehicle arrive at stop point based on localization #### Subscribed Topics -| Name | Type | Description | -| ---------------------------------------- | ---------------------------------------------- | ---------------- | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | -| `/planning/scenario_planning/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | trajectory | +| Name | Type | Description | +| ---------------------------------------- | ----------------------------------------- | ---------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | +| `/planning/scenario_planning/trajectory` | `autoware_planning_msgs::msg::Trajectory` | trajectory | #### Parameters diff --git a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp index fdfd3a25eb3ad..077c66f9dd6bc 100644 --- a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp +++ b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -40,10 +39,10 @@ class VelocityFactorInterface void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; } void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; } + template void set( - const std::vector & points, - const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, - const std::string & detail = ""); + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail = ""); private: VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index 258386ca236a3..91322360ce5b8 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -15,9 +15,9 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -113,8 +113,8 @@ std::vector resamplePoseVector( * longitudinal and lateral velocity. Otherwise, it uses linear interpolation * @return resampled path */ -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); @@ -136,11 +136,10 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * @param resample_input_path_stop_point If true, resample closest stop point in input path * @return resampled path */ -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, - const bool resample_input_path_stop_point = true); +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, + const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); /** * @brief A resampling function for a path. Note that in a default setting, position xy are @@ -159,8 +158,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * longitudinal and lateral velocity. Otherwise, it uses linear interpolation * @return resampled path */ -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); @@ -181,8 +180,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * @param resample_input_path_stop_point If true, resample closest stop point in input path * @return resampled path */ -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, const bool resample_input_path_stop_point = true); @@ -205,8 +204,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation * @return resampled trajectory */ -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true); @@ -230,10 +229,10 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( * trajectory * @return resampled trajectory */ -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, + const bool use_zero_order_hold_for_twist = true, const bool resample_input_trajectory_stop_point = true); } // namespace motion_utils diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp index 7d4be216e89fe..8d009730a925d 100644 --- a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -15,52 +15,52 @@ #ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ #define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "autoware_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include "std_msgs/msg/header.hpp" +#include "tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include namespace motion_utils { -using TrajectoryPoints = std::vector; +using TrajectoryPoints = std::vector; /** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @brief Convert std::vector to + * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_msgs. We should consider whether to remove this function after the porting is done. * @attention This function just clips - * std::vector up to the capacity of Trajectory. + * std::vector up to the capacity of Trajectory. * Therefore, the error handling out of this function is necessary if the size of the input greater * than the capacity. * @todo Decide how to handle the situation that we need to use the trajectory with the size of * points larger than the capacity. (Tier IV) */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory, +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, const std_msgs::msg::Header & header = std_msgs::msg::Header{}); /** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory); template -autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) +autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) { static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); throw std::logic_error("Only specializations of convertToPath can be used."); } template <> -inline autoware_auto_planning_msgs::msg::Path convertToPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +inline autoware_planning_msgs::msg::Path convertToPath( + const tier4_planning_msgs::msg::PathWithLaneId & input) { - autoware_auto_planning_msgs::msg::Path output{}; + autoware_planning_msgs::msg::Path output{}; output.header = input.header; output.left_bound = input.left_bound; output.right_bound = input.right_bound; @@ -80,11 +80,11 @@ TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) template <> inline TrajectoryPoints convertToTrajectoryPoints( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input) + const tier4_planning_msgs::msg::PathWithLaneId & input) { TrajectoryPoints output{}; for (const auto & p : input.points) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tp; + autoware_planning_msgs::msg::TrajectoryPoint tp; tp.pose = p.point.pose; tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; // since path point doesn't have acc for now @@ -95,20 +95,19 @@ inline TrajectoryPoints convertToTrajectoryPoints( } template -autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( - [[maybe_unused]] const T & input) +tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId([[maybe_unused]] const T & input) { static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); } template <> -inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( +inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( const TrajectoryPoints & input) { - autoware_auto_planning_msgs::msg::PathWithLaneId output{}; + tier4_planning_msgs::msg::PathWithLaneId output{}; for (const auto & p : input) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId pp; + tier4_planning_msgs::msg::PathPointWithLaneId pp; pp.point.pose = p.pose; pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; output.points.emplace_back(pp); diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index aec329f9f540e..0a0b14cb7488d 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -17,8 +17,10 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" + +#include #include #include @@ -34,8 +36,8 @@ namespace motion_utils * twist information * @return resampled path(poses) */ -autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, +autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( + const autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -49,8 +51,8 @@ autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( * twist information * @return resampled path(poses) */ -autoware_auto_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, +tier4_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index d0ed761b76d9a..d6d4fbf559323 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -15,7 +15,7 @@ #ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include #include @@ -23,23 +23,23 @@ namespace motion_utils { std::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); + const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id); size_t findNearestSegmentIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id); // @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle // center follow the input path // @param [in] path with position to be followed by the center of the vehicle // @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle // center follow the input path NOTE: rear_to_cog is supposed to be positive -autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation = true); } // namespace motion_utils diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 7e2d92c9434fb..3715f466e7684 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -56,14 +56,12 @@ void validateNonEmpty(const T & points) } } -extern template void validateNonEmpty>( - const std::vector &); -extern template void -validateNonEmpty>( - const std::vector &); -extern template void -validateNonEmpty>( - const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); /** * @brief validate a point is in a non-sharp angle between two points or not @@ -114,14 +112,14 @@ std::optional isDrivingForward(const T & points) } extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); /** * @brief checks whether a path of trajectory has forward driving direction using its longitudinal @@ -149,14 +147,14 @@ std::optional isDrivingForwardWithTwist(const T & points_with_twist) } extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); /** * @brief remove overlapping points through points container. @@ -194,17 +192,16 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) return dst; } -extern template std::vector -removeOverlapPoints>( - const std::vector & points, +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); /** @@ -237,8 +234,8 @@ std::optional searchZeroVelocityIndex( } extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx, const size_t dst_idx); /** @@ -262,8 +259,8 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist, const } extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx); /** @@ -279,8 +276,8 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist) } extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist); +searchZeroVelocityIndex>( + const std::vector & points_with_twist); /** * @brief find nearest point index through points container for a given point. @@ -310,16 +307,14 @@ size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & poin return min_idx; } -extern template size_t findNearestIndex>( - const std::vector & points, +extern template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -extern template size_t -findNearestIndex>( - const std::vector & points, +extern template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -extern template size_t -findNearestIndex>( - const std::vector & points, +extern template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); /** @@ -378,18 +373,18 @@ std::optional findNearestIndex( } extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); @@ -462,19 +457,17 @@ double calcLongitudinalOffsetToSegment( } extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); /** * @brief find nearest segment index to point. @@ -505,17 +498,16 @@ size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point return nearest_idx; } -extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +extern template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); /** @@ -557,18 +549,18 @@ std::optional findNearestSegmentIndex( } extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); @@ -631,18 +623,17 @@ double calcLateralOffset( return cross_vec(2) / segment_vec.norm(); } -extern template double calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); extern template double -calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); @@ -695,16 +686,15 @@ double calcLateralOffset( return calcLateralOffset(points, p_target, seg_idx, throw_exception); } -extern template double calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); /** @@ -738,18 +728,17 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t return dist_sum; } -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); /** * @brief Computes the partial sums of the elements in the sub-ranges of the range [src_idx, @@ -789,17 +778,17 @@ std::vector calcSignedArcLengthPartialSum( } extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); /** * @brief calculate length of 2D distance between two points, specified by start point and end point @@ -831,17 +820,16 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector &, +calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector &, +calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); /** @@ -868,18 +856,17 @@ double calcSignedArcLength( return -calcSignedArcLength(points, dst_point, src_idx); } -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); /** * @brief calculate length of 2D distance between two points, specified by start point and end @@ -916,17 +903,16 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); /** @@ -947,14 +933,12 @@ double calcArcLength(const T & points) return calcSignedArcLength(points, 0, points.size() - 1); } -extern template double calcArcLength>( - const std::vector & points); -extern template double -calcArcLength>( - const std::vector & points); -extern template double -calcArcLength>( - const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); /** * @brief calculate curvature through points container. @@ -986,14 +970,14 @@ std::vector calcCurvature(const T & points) } extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); /** * @brief calculate curvature through points container and length of 2d distance for segment used @@ -1026,14 +1010,14 @@ std::vector> calcCurvatureAndArcLength(const T & point } extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); /** * @brief calculate length of 2D distance between given start point index in points container and @@ -1063,8 +1047,8 @@ std::optional calcDistanceToForwardStopPoint( } extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const size_t src_idx = 0); /** @@ -1139,17 +1123,17 @@ std::optional calcLongitudinalOffsetPoint( } extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception = false); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception = false); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); /** * @brief calculate the point offset from source point along the trajectory (or path) (points @@ -1184,16 +1168,16 @@ std::optional calcLongitudinalOffsetPoint( } extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); /** @@ -1284,20 +1268,20 @@ std::optional calcLongitudinalOffsetPose( } extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false); +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false); +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); /** * @brief calculate the point offset from source point along the trajectory (or path) (points @@ -1331,18 +1315,18 @@ std::optional calcLongitudinalOffsetPose( } extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); @@ -1433,19 +1417,19 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); /** @@ -1488,19 +1472,19 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); /** @@ -1561,19 +1545,19 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); /** @@ -1614,21 +1598,21 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); @@ -1666,19 +1650,19 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); /** @@ -1715,19 +1699,19 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); /** @@ -1770,21 +1754,21 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); @@ -1817,9 +1801,9 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); /** @@ -1859,10 +1843,10 @@ std::optional insertDecelPoint( } extern template std::optional -insertDecelPoint>( +insertDecelPoint>( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, - std::vector & points_with_twist); + std::vector & points_with_twist); /** * @brief Insert orientation to each point in points container (trajectory, path, ...) @@ -1901,15 +1885,13 @@ void insertOrientation(T & points, const bool is_driving_forward) } } -extern template void insertOrientation>( - std::vector & points, const bool is_driving_forward); -extern template void -insertOrientation>( - std::vector & points, +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); -extern template void -insertOrientation>( - std::vector & points, +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); /** @@ -1968,19 +1950,18 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); @@ -2009,17 +1990,16 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); /** @@ -2047,18 +2027,17 @@ double calcSignedArcLength( return signed_length_on_traj + signed_length_dst_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); /** * @brief find first nearest point index through points container for a given pose with soft @@ -2149,20 +2128,20 @@ size_t findFirstNearestIndexWithSoftConstraints( } extern template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, +extern template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2205,20 +2184,20 @@ size_t findFirstNearestSegmentIndexWithSoftConstraints( } extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2270,18 +2249,18 @@ std::optional calcDistanceToForwardStopPoint( } extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); @@ -2309,19 +2288,19 @@ T cropForwardPoints( return points; } -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); @@ -2349,19 +2328,19 @@ T cropBackwardPoints( return points; } -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); @@ -2391,19 +2370,19 @@ T cropPoints( return cropped_points; } -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); @@ -2459,16 +2438,14 @@ double calcYawDeviation( return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw); } -extern template double calcYawDeviation>( - const std::vector & points, +extern template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double -calcYawDeviation>( - const std::vector & points, +extern template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double -calcYawDeviation>( - const std::vector & points, +extern template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); /** @@ -2495,18 +2472,16 @@ bool isTargetPointFront( return s_target - s_base > threshold; } -extern template bool isTargetPointFront>( - const std::vector & points, +extern template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); -extern template bool -isTargetPointFront>( - const std::vector & points, +extern template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); -extern template bool -isTargetPointFront>( - const std::vector & points, +extern template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); diff --git a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp index 82433d8e3c241..859bbdcc851ea 100644 --- a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp +++ b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -26,7 +26,7 @@ namespace motion_utils { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::Odometry; diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index dec5287b0a520..21471a4a6a75e 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -22,8 +22,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_planning_msgs + autoware_vehicle_msgs builtin_interfaces geometry_msgs interpolation @@ -32,6 +32,7 @@ tf2 tf2_geometry_msgs tier4_autoware_utils + tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/motion_utils/src/factor/velocity_factor_interface.cpp index eabd00fae85d6..6878c056b6f7a 100644 --- a/common/motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/motion_utils/src/factor/velocity_factor_interface.cpp @@ -15,12 +15,16 @@ #include #include +#include +#include +#include + namespace motion_utils { +template void VelocityFactorInterface::set( - const std::vector & points, - const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, - const std::string & detail) + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail) { const auto & curr_point = curr_pose.position; const auto & stop_point = stop_pose.position; @@ -32,4 +36,14 @@ void VelocityFactorInterface::set( velocity_factor_.detail = detail; } +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); + } // namespace motion_utils diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 834d07a87ea09..4a4e8dff1e4ad 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -181,8 +181,8 @@ std::vector resamplePoseVector( return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { @@ -333,13 +333,13 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( return input_path; } - autoware_auto_planning_msgs::msg::PathWithLaneId resampled_path; + tier4_planning_msgs::msg::PathWithLaneId resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { - autoware_auto_planning_msgs::msg::PathPoint path_point; + autoware_planning_msgs::msg::PathPoint path_point; path_point.pose = interpolated_pose.at(i); path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); path_point.lateral_velocity_mps = interpolated_v_lat.at(i); @@ -352,9 +352,9 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( return resampled_path; } -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) { // validate arguments @@ -363,7 +363,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( } // transform input_path - std::vector transformed_input_path( + std::vector transformed_input_path( input_path.points.size()); for (size_t i = 0; i < input_path.points.size(); ++i) { transformed_input_path.at(i) = input_path.points.at(i).point; @@ -418,8 +418,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( use_zero_order_hold_for_v); } -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { @@ -483,13 +483,13 @@ autoware_auto_planning_msgs::msg::Path resamplePath( return input_path; } - autoware_auto_planning_msgs::msg::Path resampled_path; + autoware_planning_msgs::msg::Path resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { - autoware_auto_planning_msgs::msg::PathPoint path_point; + autoware_planning_msgs::msg::PathPoint path_point; path_point.pose = interpolated_pose.at(i); path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); path_point.lateral_velocity_mps = interpolated_v_lat.at(i); @@ -500,8 +500,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( return resampled_path; } -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point) { @@ -559,8 +559,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( use_zero_order_hold_for_twist); } -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) { @@ -646,11 +646,11 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( return input_trajectory; } - autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory; + autoware_planning_msgs::msg::Trajectory resampled_trajectory; resampled_trajectory.header = input_trajectory.header; resampled_trajectory.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_trajectory.points.size(); ++i) { - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + autoware_planning_msgs::msg::TrajectoryPoint traj_point; traj_point.pose = interpolated_pose.at(i); traj_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); traj_point.lateral_velocity_mps = interpolated_v_lat.at(i); @@ -665,9 +665,9 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( return resampled_trajectory; } -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) { // validate arguments diff --git a/common/motion_utils/src/trajectory/conversion.cpp b/common/motion_utils/src/trajectory/conversion.cpp index f198003d84091..e3a221c61a2a7 100644 --- a/common/motion_utils/src/trajectory/conversion.cpp +++ b/common/motion_utils/src/trajectory/conversion.cpp @@ -19,34 +19,34 @@ namespace motion_utils { /** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @brief Convert std::vector to + * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_msgs. We should consider whether to remove this function after the porting is done. * @attention This function just clips - * std::vector up to the capacity of Trajectory. + * std::vector up to the capacity of Trajectory. * Therefore, the error handling out of this function is necessary if the size of the input greater * than the capacity. * @todo Decide how to handle the situation that we need to use the trajectory with the size of * points larger than the capacity. (Tier IV) */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory, +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, const std_msgs::msg::Header & header) { - autoware_auto_planning_msgs::msg::Trajectory output{}; + autoware_planning_msgs::msg::Trajectory output{}; output.header = header; for (const auto & pt : trajectory) output.points.push_back(pt); return output; } /** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory) { - std::vector output(trajectory.points.size()); + std::vector output(trajectory.points.size()); std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); return output; } diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp index 6ee3e665f746a..13df39daab59d 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -17,10 +17,10 @@ #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; namespace motion_utils { diff --git a/common/motion_utils/src/trajectory/path_with_lane_id.cpp b/common/motion_utils/src/trajectory/path_with_lane_id.cpp index c7e85554dbddd..4c86135811003 100644 --- a/common/motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/motion_utils/src/trajectory/path_with_lane_id.cpp @@ -25,7 +25,7 @@ namespace motion_utils { std::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error @@ -56,8 +56,8 @@ std::optional> getPathIndexRangeWithLaneId( } size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id) { const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); if (opt_range) { @@ -66,7 +66,7 @@ size_t findNearestIndexFromLaneId( validateNonEmpty(path.points); - const auto sub_points = std::vector{ + const auto sub_points = std::vector{ path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; validateNonEmpty(sub_points); @@ -77,8 +77,8 @@ size_t findNearestIndexFromLaneId( } size_t findNearestSegmentIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id) { const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); @@ -99,8 +99,8 @@ size_t findNearestSegmentIndexFromLaneId( } // NOTE: rear_to_cog is supposed to be positive -autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation) { auto cog_path = path; diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/motion_utils/src/trajectory/trajectory.cpp index 51c07a75076c8..42c750e3c109f 100644 --- a/common/motion_utils/src/trajectory/trajectory.cpp +++ b/common/motion_utils/src/trajectory/trajectory.cpp @@ -18,607 +18,584 @@ namespace motion_utils { // -template void validateNonEmpty>( - const std::vector &); -template void validateNonEmpty>( - const std::vector &); -template void validateNonEmpty>( - const std::vector &); +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); // +template std::optional isDrivingForward>( + const std::vector &); template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); template std::optional -isDrivingForward>( - const std::vector &); -template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); // template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); // -template std::vector -removeOverlapPoints>( - const std::vector & points, const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); // template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx, const size_t dst_idx); // template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx); // template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist); +searchZeroVelocityIndex>( + const std::vector & points_with_twist); // -template size_t findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t -findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); // template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); // template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); // -template size_t findNearestSegmentIndex>( - const std::vector & points, +template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t -findNearestSegmentIndex>( - const std::vector & points, +template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t -findNearestSegmentIndex>( - const std::vector & points, +template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); // template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); // -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double -calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); // -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double -calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); // -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); // template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector &, +template double calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector &, +template double calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); // -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double -calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); // -template double calcArcLength>( - const std::vector & points); -template double calcArcLength>( - const std::vector & points); -template double calcArcLength>( - const std::vector & points); +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); // +template std::vector calcCurvature>( + const std::vector & points); template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); template std::vector -calcCurvature>( - const std::vector & points); -template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); // template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); // template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const size_t src_idx); // template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); // template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); // template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); // template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold); + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, + std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist, const double max_yaw, const double overlap_threshold); + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, + std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); // -template std::optional -insertStopPoint>( +template std::optional insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); // -template std::optional -insertStopPoint>( +template std::optional insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); // -template std::optional -insertStopPoint>( +template std::optional insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist, const double max_yaw, const double overlap_threshold); + std::vector & points_with_twist, const double max_dist, + const double max_yaw, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); // template std::optional -insertStopPoint>( +insertStopPoint>( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); // template std::optional -insertDecelPoint>( +insertDecelPoint>( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, - std::vector & points_with_twist); + std::vector & points_with_twist); // -template void insertOrientation>( - std::vector & points, const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double -calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); // -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); // template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, +template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); // template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); // template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); // -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); // -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); // -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); // -template double calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double -calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); // -template bool isTargetPointFront>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); -template bool -isTargetPointFront>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); -template bool isTargetPointFront>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/motion_utils/test/src/resample/test_resample.cpp index 373a12c5bbd76..ef374f0b484db 100644 --- a/common/motion_utils/test/src/resample/test_resample.cpp +++ b/common/motion_utils/test/src/resample/test_resample.cpp @@ -27,15 +27,15 @@ namespace { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -297,7 +297,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -403,7 +403,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Duplicated points in the original path { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(11); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -437,7 +437,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -470,7 +470,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -504,7 +504,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -543,7 +543,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) using motion_utils::resamplePath; { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -651,7 +651,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // change initial orientation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -775,7 +775,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp x, y { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -852,7 +852,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -931,7 +931,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp v { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -1626,7 +1626,7 @@ TEST(resample_path, resample_path_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -1708,7 +1708,7 @@ TEST(resample_path, resample_path_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = @@ -1742,7 +1742,7 @@ TEST(resample_path, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -1776,7 +1776,7 @@ TEST(resample_path, resample_path_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -1815,7 +1815,7 @@ TEST(resample_path, resample_path_by_vector_backward) using motion_utils::resamplePath; { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); @@ -1896,7 +1896,7 @@ TEST(resample_path, resample_path_by_vector_backward) // change initial orientation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); @@ -1994,7 +1994,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Lerp x, y { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2053,7 +2053,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -2115,7 +2115,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Lerp v { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2179,7 +2179,7 @@ TEST(resample_path, resample_path_by_same_interval) // Same point resampling { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2241,7 +2241,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2269,7 +2269,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without stop point but with terminal point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2314,7 +2314,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without stop point but with terminal point (Boundary Condition) { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2360,7 +2360,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case with duplicated zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2390,7 +2390,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case with zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2532,7 +2532,7 @@ TEST(resample_path, resample_path_by_same_interval) { // Input path size is not enough for resample { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = @@ -2565,7 +2565,7 @@ TEST(resample_path, resample_path_by_same_interval) // Resample interval is invalid { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -2598,7 +2598,7 @@ TEST(resample_path, resample_path_by_same_interval) // Resample interval is invalid (Negative value) { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -2698,7 +2698,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2787,7 +2787,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = @@ -2821,7 +2821,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2855,7 +2855,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2895,7 +2895,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Lerp x, y { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2959,7 +2959,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = generateTestTrajectoryPoint( @@ -3025,7 +3025,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Lerp twist { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3094,7 +3094,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Same point resampling { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3160,7 +3160,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3190,7 +3190,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without stop point but with terminal point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3238,7 +3238,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without stop point but with terminal point (Boundary Condition) { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3287,7 +3287,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case with duplicated zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3319,7 +3319,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case with zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3470,7 +3470,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) { // Input path size is not enough for resample { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = @@ -3503,7 +3503,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Resample interval is invalid { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3536,7 +3536,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Resample interval is invalid (Negative value) { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = diff --git a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index 549ca4c0ae5bb..7aae860b76533 100644 --- a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -22,7 +22,7 @@ namespace { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp index 5794ed61e9e44..90b11e535c486 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -27,13 +27,13 @@ namespace { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -127,7 +127,7 @@ TEST(Interpolation, interpolate_path_for_trajectory) using motion_utils::calcInterpolatedPoint; { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -319,7 +319,7 @@ TEST(Interpolation, interpolate_path_for_trajectory) // Duplicated Points { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -351,7 +351,7 @@ TEST(Interpolation, interpolate_path_for_path) using motion_utils::calcInterpolatedPoint; { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -515,7 +515,7 @@ TEST(Interpolation, interpolate_path_for_path) // Duplicated Points { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -543,7 +543,7 @@ TEST(Interpolation, interpolate_points_with_length) using motion_utils::calcInterpolatedPose; { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -631,7 +631,7 @@ TEST(Interpolation, interpolate_points_with_length) // one point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = generateTestTrajectoryPoint( diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index a65147a65b12d..6dd60c499d181 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -22,9 +22,9 @@ namespace { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::createPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) @@ -54,8 +54,8 @@ PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { - using autoware_auto_planning_msgs::msg::PathWithLaneId; using motion_utils::getPathIndexRangeWithLaneId; + using tier4_planning_msgs::msg::PathWithLaneId; // Usual cases { diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 8e84b111b0688..9ebc712b3bf1a 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -26,8 +26,8 @@ namespace { -using autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPointArray = std::vector; +using autoware_planning_msgs::msg::Trajectory; +using TrajectoryPointArray = std::vector; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; @@ -69,7 +69,7 @@ TrajectoryPointArray generateTestTrajectoryPointArray( const size_t num_points, const double point_interval, const double vel = 0.0, const double init_theta = 0.0, const double delta_theta = 0.0) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; TrajectoryPointArray traj; for (size_t i = 0; i < num_points; ++i) { const double theta = init_theta + i * delta_theta; @@ -106,7 +106,7 @@ TEST(trajectory, validateNonEmpty) TEST(trajectory, validateNonSharpAngle_DefaultThreshold) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::validateNonSharpAngle; TrajectoryPoint p1; @@ -135,7 +135,7 @@ TEST(trajectory, validateNonSharpAngle_DefaultThreshold) TEST(trajectory, validateNonSharpAngle_SetThreshold) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::validateNonSharpAngle; using tier4_autoware_utils::pi; @@ -1299,7 +1299,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::deg2rad; @@ -1385,7 +1385,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::deg2rad; @@ -1563,7 +1563,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; @@ -1637,7 +1637,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; @@ -5393,7 +5393,7 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) TEST(trajectory, calcYawDeviation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcYawDeviation; constexpr double tolerance = 1e-3; @@ -5420,7 +5420,7 @@ TEST(trajectory, calcYawDeviation) TEST(trajectory, isTargetPointFront) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::isTargetPointFront; using tier4_autoware_utils::createPoint; diff --git a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp index aa26a64cb87ce..29802e70bfd5f 100644 --- a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp +++ b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp @@ -15,11 +15,11 @@ #ifndef VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ #define VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ -#include -#include +#include +#include -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; inline Trajectory generateTrajectoryWithStopPoint(const geometry_msgs::msg::Pose & goal_pose) { diff --git a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp b/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp index c6dd0104b1e11..6d07d161bf069 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp @@ -15,15 +15,15 @@ #ifndef OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ #define OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ -#include -#include +#include +#include namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; DetectedObject toDetectedObject(const TrackedObject & tracked_object); DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects); diff --git a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp b/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp index ae3013c728032..28cc9a786ecac 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp @@ -15,9 +15,9 @@ #ifndef OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ #define OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ -#include -#include -#include +#include +#include +#include #include namespace object_recognition_utils @@ -36,22 +36,19 @@ inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::Pose & p) } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::DetectedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::DetectedObject & obj) { return obj.kinematics.pose_with_covariance.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::TrackedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::TrackedObject & obj) { return obj.kinematics.pose_with_covariance.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::PredictedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::PredictedObject & obj) { return obj.kinematics.initial_pose_with_covariance.pose; } diff --git a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp b/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp index 9c0745bb28af9..4ffafc22339d6 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp @@ -15,14 +15,14 @@ #ifndef OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ #define OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" #include #include namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::ObjectClassification; inline ObjectClassification getHighestProbClassification( const std::vector & object_classifications) diff --git a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp index 73cdce6c6444e..1cd838e090560 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp @@ -17,7 +17,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include #include #include @@ -34,7 +34,7 @@ namespace object_recognition_utils * @return interpolated pose */ boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time); + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time); /** * @brief Resampling predicted path by time step vector. Note this function does not substitute @@ -44,8 +44,8 @@ boost::optional calcInterpolatedPose( * time_step*(num_of_path_points)] * @return resampled path */ -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy = true, const bool use_spline_for_z = false); @@ -57,10 +57,10 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( * @param sampling_horizon sampling time horizon * @return resampled path */ -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, - const double sampling_time_interval, const double sampling_horizon, - const bool use_spline_for_xy = true, const bool use_spline_for_z = false); +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy = true, + const bool use_spline_for_z = false); } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 796e276c376d6..fdf73e6b26057 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs interpolation libboost-dev diff --git a/common/object_recognition_utils/src/conversion.cpp b/common/object_recognition_utils/src/conversion.cpp index f6a25cee2c7d5..c8b8e6585f5a7 100644 --- a/common/object_recognition_utils/src/conversion.cpp +++ b/common/object_recognition_utils/src/conversion.cpp @@ -16,10 +16,10 @@ namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; DetectedObject toDetectedObject(const TrackedObject & tracked_object) { @@ -43,7 +43,7 @@ DetectedObject toDetectedObject(const TrackedObject & tracked_object) DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects) { - autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; + autoware_perception_msgs::msg::DetectedObjects detected_objects; detected_objects.header = tracked_objects.header; for (auto & tracked_object : tracked_objects.objects) { diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index 81a22c98d88e5..fe9499b4eec33 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -23,7 +23,7 @@ namespace object_recognition_utils { boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time) + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time) { // Check if relative time is in the valid range if (path.path.empty() || relative_time < 0.0) { @@ -45,8 +45,8 @@ boost::optional calcInterpolatedPose( return boost::none; } -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy, const bool use_spline_for_z) { @@ -83,7 +83,7 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( const auto interpolated_z = use_spline_for_z ? spline(z) : lerp(z); const auto interpolated_quat = slerp(quat); - autoware_auto_perception_msgs::msg::PredictedPath resampled_path; + autoware_perception_msgs::msg::PredictedPath resampled_path; const auto resampled_size = std::min(resampled_path.path.max_size(), resampled_time.size()); resampled_path.confidence = path.confidence; resampled_path.path.resize(resampled_size); @@ -99,10 +99,9 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( return resampled_path; } -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, - const double sampling_time_interval, const double sampling_horizon, const bool use_spline_for_xy, - const bool use_spline_for_z) +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy, const bool use_spline_for_z) { if (path.path.empty()) { throw std::invalid_argument("Predicted Path is empty"); diff --git a/common/object_recognition_utils/test/src/test_conversion.cpp b/common/object_recognition_utils/test/src/test_conversion.cpp index 61fa0efd07d04..378f9a1245746 100644 --- a/common/object_recognition_utils/test/src/test_conversion.cpp +++ b/common/object_recognition_utils/test/src/test_conversion.cpp @@ -28,10 +28,10 @@ geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const return p; } -autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassification( +autoware_perception_msgs::msg::ObjectClassification createObjectClassification( const std::uint8_t label, const double probability) { - autoware_auto_perception_msgs::msg::ObjectClassification classification; + autoware_perception_msgs::msg::ObjectClassification classification; classification.label = label; classification.probability = probability; @@ -42,10 +42,10 @@ autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassificat // NOTE: covariance is not checked TEST(conversion, test_toDetectedObject) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toDetectedObject; - autoware_auto_perception_msgs::msg::TrackedObject tracked_obj; + autoware_perception_msgs::msg::TrackedObject tracked_obj; // existence probability tracked_obj.existence_probability = 1.0; // classification @@ -160,10 +160,10 @@ TEST(conversion, test_toDetectedObject) // NOTE: covariance is not checked TEST(conversion, test_toTrackedObject) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toTrackedObject; - autoware_auto_perception_msgs::msg::DetectedObject detected_obj; + autoware_perception_msgs::msg::DetectedObject detected_obj; // existence probability detected_obj.existence_probability = 1.0; // classification diff --git a/common/object_recognition_utils/test/src/test_geometry.cpp b/common/object_recognition_utils/test/src/test_geometry.cpp index fe9f61b2424c6..ab3ba8fcec7d4 100644 --- a/common/object_recognition_utils/test/src/test_geometry.cpp +++ b/common/object_recognition_utils/test/src/test_geometry.cpp @@ -50,7 +50,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::DetectedObject p; + autoware_perception_msgs::msg::DetectedObject p; p.kinematics.pose_with_covariance.pose.position.x = x_ans; p.kinematics.pose_with_covariance.pose.position.y = y_ans; p.kinematics.pose_with_covariance.pose.position.z = z_ans; @@ -70,7 +70,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::TrackedObject p; + autoware_perception_msgs::msg::TrackedObject p; p.kinematics.pose_with_covariance.pose.position.x = x_ans; p.kinematics.pose_with_covariance.pose.position.y = y_ans; p.kinematics.pose_with_covariance.pose.position.z = z_ans; @@ -90,7 +90,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::PredictedObject p; + autoware_perception_msgs::msg::PredictedObject p; p.kinematics.initial_pose_with_covariance.pose.position.x = x_ans; p.kinematics.initial_pose_with_covariance.pose.position.y = y_ans; p.kinematics.initial_pose_with_covariance.pose.position.z = z_ans; diff --git a/common/object_recognition_utils/test/src/test_matching.cpp b/common/object_recognition_utils/test/src/test_matching.cpp index 8603dd54c433c..2ad34c3ae6bbe 100644 --- a/common/object_recognition_utils/test/src/test_matching.cpp +++ b/common/object_recognition_utils/test/src/test_matching.cpp @@ -15,7 +15,7 @@ #include "object_recognition_utils/matching.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include #include @@ -37,7 +37,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double TEST(matching, test_get2dIoU) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dIoU; const double quart_circle = 0.16237976320958225; @@ -45,13 +45,13 @@ TEST(matching, test_get2dIoU) { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -61,13 +61,13 @@ TEST(matching, test_get2dIoU) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -77,13 +77,13 @@ TEST(matching, test_get2dIoU) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -103,15 +103,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) const double quart_circle = 0.16237976320958225; { // non overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -119,15 +119,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) } { // partially overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -135,15 +135,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) } { // fully overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -153,20 +153,20 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) TEST(matching, test_get2dPrecision) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dPrecision; const double quart_circle = 0.16237976320958225; { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -180,13 +180,13 @@ TEST(matching, test_get2dPrecision) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -200,13 +200,13 @@ TEST(matching, test_get2dPrecision) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -220,20 +220,20 @@ TEST(matching, test_get2dPrecision) TEST(matching, test_get2dRecall) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dRecall; const double quart_circle = 0.16237976320958225; { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); @@ -247,13 +247,13 @@ TEST(matching, test_get2dRecall) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); @@ -267,13 +267,13 @@ TEST(matching, test_get2dRecall) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); diff --git a/common/object_recognition_utils/test/src/test_object_classification.cpp b/common/object_recognition_utils/test/src/test_object_classification.cpp index 1637dc16346cd..697a7e8447732 100644 --- a/common/object_recognition_utils/test/src/test_object_classification.cpp +++ b/common/object_recognition_utils/test/src/test_object_classification.cpp @@ -20,10 +20,10 @@ constexpr double epsilon = 1e-06; namespace { -autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassification( +autoware_perception_msgs::msg::ObjectClassification createObjectClassification( const std::uint8_t label, const double probability) { - autoware_auto_perception_msgs::msg::ObjectClassification classification; + autoware_perception_msgs::msg::ObjectClassification classification; classification.label = label; classification.probability = probability; @@ -34,17 +34,17 @@ autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassificat TEST(object_classification, test_getHighestProbLabel) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::getHighestProbLabel; { // empty - std::vector classifications; + std::vector classifications; std::uint8_t label = getHighestProbLabel(classifications); EXPECT_EQ(label, ObjectClassification::UNKNOWN); } { // normal case - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -54,7 +54,7 @@ TEST(object_classification, test_getHighestProbLabel) } { // labels with the same probability - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -67,7 +67,7 @@ TEST(object_classification, test_getHighestProbLabel) // Test isVehicle TEST(object_classification, test_isVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isVehicle; { // True Case with uint8_t @@ -87,7 +87,7 @@ TEST(object_classification, test_isVehicle) // True Case with object_classifications { // normal case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -96,7 +96,7 @@ TEST(object_classification, test_isVehicle) // False Case with object_classifications { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::PEDESTRIAN, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.7)); EXPECT_FALSE(isVehicle(classification)); @@ -106,7 +106,7 @@ TEST(object_classification, test_isVehicle) // TEST isCarLikeVehicle TEST(object_classification, test_isCarLikeVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isCarLikeVehicle; { // True Case with uint8_t @@ -126,7 +126,7 @@ TEST(object_classification, test_isCarLikeVehicle) // True Case with object_classifications { // normal case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.7)); @@ -135,7 +135,7 @@ TEST(object_classification, test_isCarLikeVehicle) // False Case with object_classifications { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.8)); EXPECT_FALSE(isCarLikeVehicle(classification)); @@ -146,7 +146,7 @@ TEST(object_classification, test_isCarLikeVehicle) // getHighestProbLabel() returns only first highest-scored label. // so, in edge case it returns a label earlier added. { // When car and non-car label has same probability - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); EXPECT_FALSE( @@ -157,7 +157,7 @@ TEST(object_classification, test_isCarLikeVehicle) // TEST isLargeVehicle TEST(object_classification, test_isLargeVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isLargeVehicle; { // True Case with uint8_t @@ -176,7 +176,7 @@ TEST(object_classification, test_isLargeVehicle) } { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); @@ -188,7 +188,7 @@ TEST(object_classification, test_isLargeVehicle) // getHighestProbLabel() returns only first highest-scored label. // so, in edge case it returns a label earlier added. { // When large-vehicle and non-large-vehicle label has same probability - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); EXPECT_TRUE(isLargeVehicle(classification)); // evaluated with earlier appended "BUS" label @@ -197,18 +197,18 @@ TEST(object_classification, test_isLargeVehicle) TEST(object_classification, test_getHighestProbClassification) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::getHighestProbClassification; { // empty - std::vector classifications; + std::vector classifications; auto classification = getHighestProbClassification(classifications); EXPECT_EQ(classification.label, ObjectClassification::UNKNOWN); EXPECT_DOUBLE_EQ(classification.probability, 0.0); } { // normal case - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -219,7 +219,7 @@ TEST(object_classification, test_getHighestProbClassification) } { // labels with the same probability - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -232,7 +232,7 @@ TEST(object_classification, test_getHighestProbClassification) TEST(object_classification, test_fromString) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toLabel; using object_recognition_utils::toObjectClassification; using object_recognition_utils::toObjectClassifications; @@ -266,7 +266,7 @@ TEST(object_classification, test_fromString) TEST(object_classification, test_convertLabelToString) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::convertLabelToString; // from label @@ -290,7 +290,7 @@ TEST(object_classification, test_convertLabelToString) // from ObjectClassifications { - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp index b466273f4defd..3dc3a8bcba3f7 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -25,7 +25,7 @@ constexpr double epsilon = 1e-06; namespace { -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedPath; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; @@ -174,7 +174,7 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) } } - // Resample which exceeds the maximum size + // Resample the path with more than 100 points { std::vector resampling_vec(101); for (size_t i = 0; i < 101; ++i) { @@ -183,10 +183,9 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) const auto resampled_path = resamplePredictedPath(path, resampling_vec); - EXPECT_EQ(resampled_path.path.size(), resampled_path.path.max_size()); EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon); - for (size_t i = 0; i < resampled_path.path.max_size(); ++i) { + for (size_t i = 0; i < resampled_path.path.size(); ++i) { EXPECT_NEAR(resampled_path.path.at(i).position.x, resampling_vec.at(i), epsilon); EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); diff --git a/common/path_distance_calculator/Readme.md b/common/path_distance_calculator/Readme.md index acbdc6998c46f..fed0476047ba9 100644 --- a/common/path_distance_calculator/Readme.md +++ b/common/path_distance_calculator/Readme.md @@ -11,10 +11,10 @@ Note that the distance means the arc-length along the path, not the Euclidean di ### Input -| Name | Type | Description | -| ----------------------------------------------------------------- | ---------------------------------------- | -------------- | -| `/planning/scenario_planning/lane_driving/behavior_planning/path` | `autoware_auto_planning_msgs::msg::Path` | Reference path | -| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | +| Name | Type | Description | +| ----------------------------------------------------------------- | ----------------------------------- | -------------- | +| `/planning/scenario_planning/lane_driving/behavior_planning/path` | `autoware_planning_msgs::msg::Path` | Reference path | +| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | ### Output diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index e796bbd0d20f7..0ccc6419b13de 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -10,7 +10,7 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs motion_utils rclcpp rclcpp_components diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index aa20dd1ffc7ce..798f5df70c596 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -25,9 +25,9 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & options) : Node("path_distance_calculator", options), self_pose_listener_(this) { - sub_path_ = create_subscription( + sub_path_ = create_subscription( "~/input/path", rclcpp::QoS(1), - [this](const autoware_auto_planning_msgs::msg::Path::SharedPtr msg) { path_ = msg; }); + [this](const autoware_planning_msgs::msg::Path::SharedPtr msg) { path_ = msg; }); pub_dist_ = create_publisher("~/output/distance", rclcpp::QoS(1)); diff --git a/common/path_distance_calculator/src/path_distance_calculator.hpp b/common/path_distance_calculator/src/path_distance_calculator.hpp index f709701024f51..838afb903c55f 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.hpp +++ b/common/path_distance_calculator/src/path_distance_calculator.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include class PathDistanceCalculator : public rclcpp::Node @@ -27,11 +27,11 @@ class PathDistanceCalculator : public rclcpp::Node explicit PathDistanceCalculator(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_path_; + rclcpp::Subscription::SharedPtr sub_path_; rclcpp::Publisher::SharedPtr pub_dist_; rclcpp::TimerBase::SharedPtr timer_; tier4_autoware_utils::SelfPoseListener self_pose_listener_; - autoware_auto_planning_msgs::msg::Path::SharedPtr path_; + autoware_planning_msgs::msg::Path::SharedPtr path_; }; #endif // PATH_DISTANCE_CALCULATOR_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 5d9001affa308..665e959ed2460 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -17,9 +17,9 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -36,14 +36,16 @@ geometry_msgs::msg::Polygon rotatePolygon( /// @return rotated polygon Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle); Polygon2d toPolygon2d( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::DetectedObject & object); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::TrackedObject & object); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object); + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape); +Polygon2d toPolygon2d( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::DetectedObject & object); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::TrackedObject & object); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::PredictedObject & object); Polygon2d toFootprint( const geometry_msgs::msg::Pose & base_link_pose, const double base_to_front, const double base_to_rear, const double width); -double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); +double getArea(const autoware_perception_msgs::msg::Shape & shape); Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset); } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 63cf6305e8158..334df8f32af91 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -27,9 +27,8 @@ #define EIGEN_MPL2_ONLY #include -#include -#include -#include +#include +#include #include #include #include @@ -39,6 +38,7 @@ #include #include #include +#include #include @@ -129,21 +129,19 @@ inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseWithCova } template <> -inline geometry_msgs::msg::Point getPoint(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::PathPoint & p) { return p.pose.position; } template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Point getPoint(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose.position; } template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.pose.position; } @@ -168,20 +166,19 @@ inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::PoseStamped & } template <> -inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::PathPoint & p) { return p.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Pose getPose(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose; } template <> -inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.pose; } @@ -194,20 +191,19 @@ double getLongitudinalVelocity([[maybe_unused]] const T & p) } template <> -inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::PathPoint & p) { return p.longitudinal_velocity_mps; } template <> -inline double getLongitudinalVelocity( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline double getLongitudinalVelocity(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.longitudinal_velocity_mps; } template <> -inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.longitudinal_velocity_mps; } @@ -233,21 +229,21 @@ inline void setPose(const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::P template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPoint & p) + const geometry_msgs::msg::Pose & pose, autoware_planning_msgs::msg::PathPoint & p) { p.pose = pose; } template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) + const geometry_msgs::msg::Pose & pose, tier4_planning_msgs::msg::PathPointWithLaneId & p) { p.point.pose = pose; } template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const geometry_msgs::msg::Pose & pose, autoware_planning_msgs::msg::TrajectoryPoint & p) { p.pose = pose; } @@ -269,21 +265,21 @@ void setLongitudinalVelocity([[maybe_unused]] const float velocity, [[maybe_unus template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const float velocity, autoware_planning_msgs::msg::TrajectoryPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::PathPoint & p) + const float velocity, autoware_planning_msgs::msg::PathPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) + const float velocity, tier4_planning_msgs::msg::PathPointWithLaneId & p) { p.point.longitudinal_velocity_mps = velocity; } diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp index 0c53a9e3941dd..4901e28472acb 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp @@ -15,6 +15,8 @@ #ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ #define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ +#include + namespace tier4_autoware_utils { @@ -22,6 +24,8 @@ float sin(float radian); float cos(float radian); +std::pair sin_and_cos(float radian); + } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index c34d3b5fdfdd0..334ee226a5f22 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -12,10 +12,10 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs autoware_internal_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_vehicle_msgs builtin_interfaces diagnostic_msgs geometry_msgs @@ -26,6 +26,7 @@ tf2 tf2_geometry_msgs tier4_debug_msgs + tier4_planning_msgs unique_identifier_msgs visualization_msgs diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp index 79c9f9937cd4d..01e02d1cf0038 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -118,11 +118,11 @@ Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle) } Polygon2d toPolygon2d( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape) + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape) { Polygon2d polygon; - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const auto point0 = tier4_autoware_utils::calcOffsetPose( pose, shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) .position; @@ -140,7 +140,7 @@ Polygon2d toPolygon2d( appendPointToPolygon(polygon, point1); appendPointToPolygon(polygon, point2); appendPointToPolygon(polygon, point3); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { const double radius = shape.dimensions.x / 2.0; constexpr int circle_discrete_num = 6; for (int i = 0; i < circle_discrete_num; ++i) { @@ -157,7 +157,7 @@ Polygon2d toPolygon2d( pose.position.y; appendPointToPolygon(polygon, point); } - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { const double poly_yaw = tf2::getYaw(pose.orientation); const auto rotated_footprint = rotatePolygon(shape.footprint, poly_yaw); for (const auto rel_point : rotated_footprint.points) { @@ -180,21 +180,21 @@ Polygon2d toPolygon2d( } tier4_autoware_utils::Polygon2d toPolygon2d( - const autoware_auto_perception_msgs::msg::DetectedObject & object) + const autoware_perception_msgs::msg::DetectedObject & object) { return tier4_autoware_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } tier4_autoware_utils::Polygon2d toPolygon2d( - const autoware_auto_perception_msgs::msg::TrackedObject & object) + const autoware_perception_msgs::msg::TrackedObject & object) { return tier4_autoware_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } tier4_autoware_utils::Polygon2d toPolygon2d( - const autoware_auto_perception_msgs::msg::PredictedObject & object) + const autoware_perception_msgs::msg::PredictedObject & object) { return tier4_autoware_utils::toPolygon2d( object.kinematics.initial_pose_with_covariance.pose, object.shape); @@ -223,13 +223,13 @@ Polygon2d toFootprint( return isClockwise(polygon) ? polygon : inverseClockwise(polygon); } -double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) +double getArea(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return getRectangleArea(shape.dimensions); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return getCircleArea(shape.dimensions); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { return getPolygonArea(shape.footprint); } diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/tier4_autoware_utils/src/math/trigonometry.cpp index 15f5c71012722..0ce65c7aa5bc8 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/tier4_autoware_utils/src/math/trigonometry.cpp @@ -49,4 +49,27 @@ float cos(float radian) return sin(radian + static_cast(tier4_autoware_utils::pi) / 2.f); } +std::pair sin_and_cos(float radian) +{ + constexpr float tmp = + (180.f / static_cast(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f); + const float degree = radian * tmp; + size_t idx = + (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % + discrete_arcs_num_360; + + if (idx < discrete_arcs_num_90) { + return {g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; + } else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) { + idx = 2 * discrete_arcs_num_90 - idx; + return {g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]}; + } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) { + idx = idx - 2 * discrete_arcs_num_90; + return {-g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]}; + } else { // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90 + idx = 4 * discrete_arcs_num_90 - idx; + return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; + } +} + } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp index 7370c3b650887..7689544bd79cc 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp @@ -137,8 +137,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double y = 1.0; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = x; shape.dimensions.y = y; @@ -159,8 +159,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double diameter = 1.0; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; shape.dimensions.x = diameter; const auto poly = toPolygon2d(pose, shape); @@ -183,8 +183,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double y = 0.5; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::POLYGON; shape.footprint.points.push_back(createPoint32(-x, -y)); shape.footprint.points.push_back(createPoint32(-x, y)); shape.footprint.points.push_back(createPoint32(x, y)); @@ -240,8 +240,8 @@ TEST(boost_geometry, boost_getArea) const double x = 1.0; const double y = 2.0; - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = x; shape.dimensions.y = y; @@ -252,8 +252,8 @@ TEST(boost_geometry, boost_getArea) { // cylinder const double diameter = 1.0; - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; shape.dimensions.x = diameter; const double area = getArea(shape); @@ -265,8 +265,8 @@ TEST(boost_geometry, boost_getArea) const double y = 2.0; // clock wise - autoware_auto_perception_msgs::msg::Shape clock_wise_shape; - clock_wise_shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape clock_wise_shape; + clock_wise_shape.type = autoware_perception_msgs::msg::Shape::POLYGON; clock_wise_shape.footprint.points.push_back(createPoint32(0.0, 0.0)); clock_wise_shape.footprint.points.push_back(createPoint32(0.0, y)); clock_wise_shape.footprint.points.push_back(createPoint32(x, y)); @@ -276,8 +276,8 @@ TEST(boost_geometry, boost_getArea) EXPECT_DOUBLE_EQ(clock_wise_area, -x * y); // anti clock wise - autoware_auto_perception_msgs::msg::Shape anti_clock_wise_shape; - anti_clock_wise_shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape anti_clock_wise_shape; + anti_clock_wise_shape.type = autoware_perception_msgs::msg::Shape::POLYGON; anti_clock_wise_shape.footprint.points.push_back(createPoint32(0.0, 0.0)); anti_clock_wise_shape.footprint.points.push_back(createPoint32(x, 0.0)); anti_clock_wise_shape.footprint.points.push_back(createPoint32(x, y)); diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index c10e04736b5bb..b492da2d72e88 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -90,7 +90,7 @@ TEST(geometry, getPoint) } { - autoware_auto_planning_msgs::msg::PathPoint p; + autoware_planning_msgs::msg::PathPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -101,7 +101,7 @@ TEST(geometry, getPoint) } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -163,7 +163,7 @@ TEST(geometry, getPose) } { - autoware_auto_planning_msgs::msg::PathPoint p; + autoware_planning_msgs::msg::PathPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -182,7 +182,7 @@ TEST(geometry, getPose) } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -208,13 +208,13 @@ TEST(geometry, getLongitudinalVelocity) const double velocity = 1.0; { - autoware_auto_planning_msgs::msg::PathPoint p; + autoware_planning_msgs::msg::PathPoint p; p.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } @@ -266,7 +266,7 @@ TEST(geometry, setPose) } { - autoware_auto_planning_msgs::msg::PathPoint p_out{}; + autoware_planning_msgs::msg::PathPoint p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); @@ -278,7 +278,7 @@ TEST(geometry, setPose) } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p_out{}; + autoware_planning_msgs::msg::TrajectoryPoint p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); @@ -313,13 +313,13 @@ TEST(geometry, setLongitudinalVelocity) const double velocity = 1.0; { - autoware_auto_planning_msgs::msg::PathPoint p{}; + autoware_planning_msgs::msg::PathPoint p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity); } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p{}; + autoware_planning_msgs::msg::TrajectoryPoint p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity); } diff --git a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp index a8179f3b3cd60..5c55a09af95b1 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -24,7 +24,7 @@ TEST(geometry, getPoint_PathWithLaneId) const double y_ans = 2.0; const double z_ans = 3.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -46,7 +46,7 @@ TEST(geometry, getPose_PathWithLaneId) const double q_z_ans = 0.3; const double q_w_ans = 0.4; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -70,7 +70,7 @@ TEST(geometry, getLongitudinalVelocity_PathWithLaneId) const double velocity = 1.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } @@ -96,7 +96,7 @@ TEST(geometry, setPose_PathWithLaneId) p.orientation.z = q_z_ans; p.orientation.w = q_w_ans; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p_out{}; + tier4_planning_msgs::msg::PathPointWithLaneId p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.point.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.point.pose.position.y, y_ans); @@ -113,7 +113,7 @@ TEST(geometry, setLongitudinalVelocity_PathWithLaneId) const double velocity = 1.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p{}; + tier4_planning_msgs::msg::PathPointWithLaneId p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.point.longitudinal_velocity_mps, velocity); } diff --git a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp index 379418539841c..d7106fd823682 100644 --- a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp +++ b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp @@ -40,3 +40,13 @@ TEST(trigonometry, cos) tier4_autoware_utils::cos(x * static_cast(i))) < 10e-5); } } + +TEST(trigonometry, sin_and_cos) +{ + float x = 4.f * tier4_autoware_utils::pi / 128.f; + for (int i = 0; i < 128; i++) { + const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast(i)); + EXPECT_TRUE(std::abs(std::sin(x * static_cast(i)) - sin_and_cos.first) < 10e-7); + EXPECT_TRUE(std::abs(std::cos(x * static_cast(i)) - sin_and_cos.second) < 10e-7); + } +} diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp index 4fc6aeb71771d..7dbbb65c94560 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp @@ -53,8 +53,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class CarInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp index fc40b08be822b..c1ec65a0488bd 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -82,8 +82,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class InteractiveObject diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp index 195ec8e2a3a6d..642159aceaf3d 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp @@ -53,8 +53,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class PedestrianInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp index 2bc3129b51321..3f1a9b55c30ab 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp @@ -48,8 +48,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class UnknownInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_planning_rviz_plugin/README.md b/common/tier4_planning_rviz_plugin/README.md index ef9df25f2b657..5b6286139af0d 100644 --- a/common/tier4_planning_rviz_plugin/README.md +++ b/common/tier4_planning_rviz_plugin/README.md @@ -11,11 +11,11 @@ This plugin displays the path, trajectory, and maximum speed. ### Input -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------------- | ------------------------------------------ | -| `/input/path` | `autoware_auto_planning_msgs::msg::Path` | The topic on which to subscribe path | -| `/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | The topic on which to subscribe trajectory | -| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------------- | ------------------------------------------ | +| `/input/path` | `autoware_planning_msgs::msg::Path` | The topic on which to subscribe path | +| `/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | The topic on which to subscribe trajectory | +| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | ### Output diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index 66b4a31f0993f..ab6a0b25fd3b4 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -17,9 +17,9 @@ #include "tier4_planning_rviz_plugin/path/display_base.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -188,7 +188,7 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay }; class AutowarePathWithLaneIdDisplay -: public AutowarePathWithDrivableAreaDisplay +: public AutowarePathWithDrivableAreaDisplay { Q_OBJECT public: @@ -198,9 +198,9 @@ class AutowarePathWithLaneIdDisplay private: void preProcessMessageDetail() override; void preVisualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; void visualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) override; rviz_common::properties::BoolProperty property_lane_id_view_; @@ -212,7 +212,7 @@ class AutowarePathWithLaneIdDisplay }; class AutowarePathDisplay -: public AutowarePathWithDrivableAreaDisplay +: public AutowarePathWithDrivableAreaDisplay { Q_OBJECT private: @@ -220,7 +220,7 @@ class AutowarePathDisplay }; class AutowareTrajectoryDisplay -: public AutowarePathBaseDisplay +: public AutowarePathBaseDisplay { Q_OBJECT private: diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index 4a59006e3bb96..ed79e122c0a01 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include @@ -316,6 +316,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); node->removeAndDestroyAllChildren(); this->scene_manager_->destroySceneNode(node); + + rviz_rendering::MovableText * text = velocity_texts_.at(i); + delete text; } velocity_texts_.resize(msg_ptr->points.size()); velocity_text_nodes_.resize(msg_ptr->points.size()); @@ -339,6 +342,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); node->removeAndDestroyAllChildren(); this->scene_manager_->destroySceneNode(node); + + rviz_rendering::MovableText * text = slope_texts_.at(i); + delete text; } slope_texts_.resize(msg_ptr->points.size()); slope_text_nodes_.resize(msg_ptr->points.size()); diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 4af4a371ef651..5d755d212c859 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -11,7 +11,6 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs autoware_planning_msgs libqt5-core libqt5-gui diff --git a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml index 0a80327199606..dac8c41670f3c 100644 --- a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml @@ -2,12 +2,12 @@ - Display path points of autoware_auto_planning_msg::Path + Display path points of autoware_planning_msg::Path - Display path_with_lane_id of autoware_auto_planning_msg::PathWithLaneId + Display path_with_lane_id of autoware_planning_msg::PathWithLaneId - Display trajectory points of autoware_auto_planning_msg::Trajectory + Display trajectory points of autoware_planning_msg::Trajectory points.size(); // clear previous text @@ -73,8 +73,7 @@ void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( } void AutowarePathWithLaneIdDisplay::visualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, - const size_t p_idx) + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) { const auto & point = msg_ptr->points.at(p_idx); diff --git a/common/tier4_state_rviz_plugin/README.md b/common/tier4_state_rviz_plugin/README.md index ac1e188fb36cd..e45be3bea13dc 100644 --- a/common/tier4_state_rviz_plugin/README.md +++ b/common/tier4_state_rviz_plugin/README.md @@ -16,7 +16,7 @@ This plugin also can engage from the panel. | `/api/localization/initialization_state` | `autoware_adapi_v1_msgs::msg::LocalizationInitializationState` | The topic represents the state of localization initialization | | `/api/motion/state` | `autoware_adapi_v1_msgs::msg::MotionState` | The topic represents the state of motion | | `/api/autoware/get/emergency` | `tier4_external_api_msgs::msg::Emergency` | The topic represents the state of external emergency | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic represents the state of gear | +| `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | The topic represents the state of gear | ### Output diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 1db152e9632f8..65ccd217d8016 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -13,8 +13,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_vehicle_msgs libqt5-core libqt5-gui libqt5-widgets diff --git a/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp index 9863cbbbf802b..c1ab9018e12f9 100644 --- a/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include #include @@ -109,12 +109,12 @@ public Q_SLOTS: // NOLINT for Qt QVBoxLayout * makeFailSafeGroup(); // QVBoxLayout * makeDiagnosticGroup(); - // void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); + // void onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; - rclcpp::Subscription::SharedPtr sub_gear_; + rclcpp::Subscription::SharedPtr sub_gear_; rclcpp::Client::SharedPtr client_emergency_stop_; rclcpp::Subscription::SharedPtr sub_emergency_; diff --git a/common/tier4_system_rviz_plugin/README.md b/common/tier4_system_rviz_plugin/README.md index 61260ecfda723..763504c4709a9 100644 --- a/common/tier4_system_rviz_plugin/README.md +++ b/common/tier4_system_rviz_plugin/README.md @@ -6,6 +6,6 @@ This plugin display the Hazard information from Autoware; and output notices whe ## Input -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------ | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ------------------------------------------------------------ | +| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | diff --git a/common/tier4_system_rviz_plugin/package.xml b/common/tier4_system_rviz_plugin/package.xml index adae997ea07aa..bd72ecf97def4 100644 --- a/common/tier4_system_rviz_plugin/package.xml +++ b/common/tier4_system_rviz_plugin/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_system_msgs + autoware_system_msgs diagnostic_msgs libqt5-core libqt5-gui diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp index 61c2bd378dab1..c0db8cc86450b 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -50,7 +50,7 @@ #include #include -#include +#include #include #include @@ -162,7 +162,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) // MRM summary std::vector mrm_comfortable_stop_summary_list = {}; std::vector mrm_emergency_stop_summary_list = {}; - int hazard_level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT; + int hazard_level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; { std::lock_guard message_lock(mutex_); if (last_msg_ptr_) { @@ -207,7 +207,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) std::ostringstream output_text; // Display the MRM Summary only when there is a fault - if (hazard_level != autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT) { + if (hazard_level != autoware_system_msgs::msg::HazardStatus::NO_FAULT) { // Broadcasting the Basic Error Infos int number_of_comfortable_stop_messages = static_cast(mrm_comfortable_stop_summary_list.size()); @@ -243,7 +243,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) } void MrmSummaryOverlayDisplay::processMessage( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp index d0b0e32c3788f..0f59ba5582fed 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp @@ -59,14 +59,14 @@ #include #include -#include +#include #endif namespace rviz_plugins { class MrmSummaryOverlayDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -85,7 +85,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; rviz_common::properties::IntProperty * property_left_; @@ -101,7 +101,7 @@ private Q_SLOTS: static constexpr int hand_width_ = 4; std::mutex mutex_; - autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; + autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_traffic_light_rviz_plugin/README.md b/common/tier4_traffic_light_rviz_plugin/README.md index 91fb5bc124cb7..cc37d17768d49 100644 --- a/common/tier4_traffic_light_rviz_plugin/README.md +++ b/common/tier4_traffic_light_rviz_plugin/README.md @@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals. ### Output -| Name | Type | Description | -| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals | +| Name | Type | Description | +| ------------------------------------------------------- | ------------------------------------------------------- | ----------------------------- | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | Publish traffic light signals | ## HowToUse diff --git a/common/tier4_traffic_light_rviz_plugin/package.xml b/common/tier4_traffic_light_rviz_plugin/package.xml index 2b118b429f1af..b123f8e2bdc3c 100644 --- a/common/tier4_traffic_light_rviz_plugin/package.xml +++ b/common/tier4_traffic_light_rviz_plugin/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs + autoware_map_msgs autoware_perception_msgs lanelet2_extension libqt5-core diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index 35c5a88a2f8a6..db33abd72283d 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -139,66 +139,66 @@ void TrafficLightPublishPanel::onSetTrafficLightState() const auto shape = light_shape_combo_->currentText(); const auto status = light_status_combo_->currentText(); - TrafficSignalElement traffic_light; + TrafficLightElement traffic_light; traffic_light.confidence = traffic_light_confidence_input_->value(); if (color == "RED") { - traffic_light.color = TrafficSignalElement::RED; + traffic_light.color = TrafficLightElement::RED; } else if (color == "AMBER") { - traffic_light.color = TrafficSignalElement::AMBER; + traffic_light.color = TrafficLightElement::AMBER; } else if (color == "GREEN") { - traffic_light.color = TrafficSignalElement::GREEN; + traffic_light.color = TrafficLightElement::GREEN; } else if (color == "WHITE") { - traffic_light.color = TrafficSignalElement::WHITE; + traffic_light.color = TrafficLightElement::WHITE; } else if (color == "UNKNOWN") { - traffic_light.color = TrafficSignalElement::UNKNOWN; + traffic_light.color = TrafficLightElement::UNKNOWN; } if (shape == "CIRCLE") { - traffic_light.shape = TrafficSignalElement::CIRCLE; + traffic_light.shape = TrafficLightElement::CIRCLE; } else if (shape == "LEFT_ARROW") { - traffic_light.shape = TrafficSignalElement::LEFT_ARROW; + traffic_light.shape = TrafficLightElement::LEFT_ARROW; } else if (shape == "RIGHT_ARROW") { - traffic_light.shape = TrafficSignalElement::RIGHT_ARROW; + traffic_light.shape = TrafficLightElement::RIGHT_ARROW; } else if (shape == "UP_ARROW") { - traffic_light.shape = TrafficSignalElement::UP_ARROW; + traffic_light.shape = TrafficLightElement::UP_ARROW; } else if (shape == "DOWN_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_ARROW; } else if (shape == "DOWN_LEFT_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_LEFT_ARROW; } else if (shape == "DOWN_RIGHT_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_RIGHT_ARROW; } else if (shape == "UNKNOWN") { - traffic_light.shape = TrafficSignalElement::UNKNOWN; + traffic_light.shape = TrafficLightElement::UNKNOWN; } if (status == "SOLID_OFF") { - traffic_light.status = TrafficSignalElement::SOLID_OFF; + traffic_light.status = TrafficLightElement::SOLID_OFF; } else if (status == "SOLID_ON") { - traffic_light.status = TrafficSignalElement::SOLID_ON; + traffic_light.status = TrafficLightElement::SOLID_ON; } else if (status == "FLASHING") { - traffic_light.status = TrafficSignalElement::FLASHING; + traffic_light.status = TrafficLightElement::FLASHING; } else if (status == "UNKNOWN") { - traffic_light.status = TrafficSignalElement::UNKNOWN; + traffic_light.status = TrafficLightElement::UNKNOWN; } - TrafficSignal traffic_signal; + TrafficLightGroup traffic_signal; traffic_signal.elements.push_back(traffic_light); - traffic_signal.traffic_signal_id = traffic_light_id; + traffic_signal.traffic_light_group_id = traffic_light_id; - for (auto & signal : extra_traffic_signals_.signals) { - if (signal.traffic_signal_id == traffic_light_id) { + for (auto & signal : extra_traffic_signals_.traffic_light_groups) { + if (signal.traffic_light_group_id == traffic_light_id) { signal = traffic_signal; return; } } - extra_traffic_signals_.signals.push_back(traffic_signal); + extra_traffic_signals_.traffic_light_groups.push_back(traffic_signal); } void TrafficLightPublishPanel::onResetTrafficLightState() { - extra_traffic_signals_.signals.clear(); + extra_traffic_signals_.traffic_light_groups.clear(); enable_publish_ = false; publish_button_->setText("PUBLISH"); @@ -218,10 +218,10 @@ void TrafficLightPublishPanel::onInitialize() using std::placeholders::_1; raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - pub_traffic_signals_ = raw_node_->create_publisher( + pub_traffic_signals_ = raw_node_->create_publisher( "/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1)); - sub_vector_map_ = raw_node_->create_subscription( + sub_vector_map_ = raw_node_->create_subscription( "/map/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&TrafficLightPublishPanel::onVectorMap, this, _1)); createWallTimer(); @@ -252,20 +252,20 @@ void TrafficLightPublishPanel::onTimer() pub_traffic_signals_->publish(extra_traffic_signals_); } - traffic_table_->setRowCount(extra_traffic_signals_.signals.size()); + traffic_table_->setRowCount(extra_traffic_signals_.traffic_light_groups.size()); - if (extra_traffic_signals_.signals.empty()) { + if (extra_traffic_signals_.traffic_light_groups.empty()) { return; } - for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) { - const auto & signal = extra_traffic_signals_.signals.at(i); + for (size_t i = 0; i < extra_traffic_signals_.traffic_light_groups.size(); ++i) { + const auto & signal = extra_traffic_signals_.traffic_light_groups.at(i); if (signal.elements.empty()) { continue; } - auto id_label = new QLabel(QString::number(signal.traffic_signal_id)); + auto id_label = new QLabel(QString::number(signal.traffic_light_group_id)); id_label->setAlignment(Qt::AlignCenter); auto color_label = new QLabel(); @@ -273,23 +273,23 @@ void TrafficLightPublishPanel::onTimer() const auto & light = signal.elements.front(); switch (light.color) { - case TrafficSignalElement::RED: + case TrafficLightElement::RED: color_label->setText("RED"); color_label->setStyleSheet("background-color: #FF0000;"); break; - case TrafficSignalElement::AMBER: + case TrafficLightElement::AMBER: color_label->setText("AMBER"); color_label->setStyleSheet("background-color: #FFBF00;"); break; - case TrafficSignalElement::GREEN: + case TrafficLightElement::GREEN: color_label->setText("GREEN"); color_label->setStyleSheet("background-color: #7CFC00;"); break; - case TrafficSignalElement::WHITE: + case TrafficLightElement::WHITE: color_label->setText("WHITE"); color_label->setStyleSheet("background-color: #FFFFFF;"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: color_label->setText("UNKNOWN"); color_label->setStyleSheet("background-color: #808080;"); break; @@ -301,28 +301,28 @@ void TrafficLightPublishPanel::onTimer() shape_label->setAlignment(Qt::AlignCenter); switch (light.shape) { - case TrafficSignalElement::CIRCLE: + case TrafficLightElement::CIRCLE: shape_label->setText("CIRCLE"); break; - case TrafficSignalElement::LEFT_ARROW: + case TrafficLightElement::LEFT_ARROW: shape_label->setText("LEFT_ARROW"); break; - case TrafficSignalElement::RIGHT_ARROW: + case TrafficLightElement::RIGHT_ARROW: shape_label->setText("RIGHT_ARROW"); break; - case TrafficSignalElement::UP_ARROW: + case TrafficLightElement::UP_ARROW: shape_label->setText("UP_ARROW"); break; - case TrafficSignalElement::DOWN_ARROW: + case TrafficLightElement::DOWN_ARROW: shape_label->setText("DOWN_ARROW"); break; - case TrafficSignalElement::DOWN_LEFT_ARROW: + case TrafficLightElement::DOWN_LEFT_ARROW: shape_label->setText("DOWN_LEFT_ARROW"); break; - case TrafficSignalElement::DOWN_RIGHT_ARROW: + case TrafficLightElement::DOWN_RIGHT_ARROW: shape_label->setText("DOWN_RIGHT_ARROW"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: shape_label->setText("UNKNOWN"); break; default: @@ -333,16 +333,16 @@ void TrafficLightPublishPanel::onTimer() status_label->setAlignment(Qt::AlignCenter); switch (light.status) { - case TrafficSignalElement::SOLID_OFF: + case TrafficLightElement::SOLID_OFF: status_label->setText("SOLID_OFF"); break; - case TrafficSignalElement::SOLID_ON: + case TrafficLightElement::SOLID_ON: status_label->setText("SOLID_ON"); break; - case TrafficSignalElement::FLASHING: + case TrafficLightElement::FLASHING: status_label->setText("FLASHING"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: status_label->setText("UNKNOWN"); break; default: @@ -361,7 +361,7 @@ void TrafficLightPublishPanel::onTimer() traffic_table_->update(); } -void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg) +void TrafficLightPublishPanel::onVectorMap(const LaneletMapBin::ConstSharedPtr msg) { if (received_vector_map_) return; // NOTE: examples from map_loader/lanelet2_map_visualization_node.cpp diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp index e54b6a301873b..2979563062fea 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp @@ -26,8 +26,8 @@ #include #include -#include -#include +#include +#include #endif #include @@ -35,10 +35,10 @@ namespace rviz_plugins { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_perception_msgs::msg::TrafficSignal; -using autoware_perception_msgs::msg::TrafficSignalArray; -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; +using autoware_perception_msgs::msg::TrafficLightGroupArray; class TrafficLightPublishPanel : public rviz_common::Panel { Q_OBJECT @@ -56,12 +56,12 @@ public Q_SLOTS: protected: void onTimer(); void createWallTimer(); - void onVectorMap(const HADMapBin::ConstSharedPtr msg); + void onVectorMap(const LaneletMapBin::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; rclcpp::TimerBase::SharedPtr pub_timer_; - rclcpp::Publisher::SharedPtr pub_traffic_signals_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Publisher::SharedPtr pub_traffic_signals_; + rclcpp::Subscription::SharedPtr sub_vector_map_; QSpinBox * publishing_rate_input_; QComboBox * traffic_light_id_input_; @@ -74,7 +74,7 @@ public Q_SLOTS: QPushButton * publish_button_; QTableWidget * traffic_table_; - TrafficSignalArray extra_traffic_signals_; + TrafficLightGroupArray extra_traffic_signals_; bool enable_publish_{false}; std::set traffic_light_ids_; diff --git a/common/tier4_vehicle_rviz_plugin/README.md b/common/tier4_vehicle_rviz_plugin/README.md index 09576ac327bec..9560963b1f5c0 100644 --- a/common/tier4_vehicle_rviz_plugin/README.md +++ b/common/tier4_vehicle_rviz_plugin/README.md @@ -11,12 +11,12 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t ### Input -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------------- | ---------------------------------- | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | -| `/control/turn_signal_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | -| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | -| `/localization/acceleration` | `geometry_msgs::msg::AccelWithCovarianceStamped` | The topic is the acceleration | +| Name | Type | Description | +| --------------------------------- | -------------------------------------------------- | ---------------------------------- | +| `/vehicle/status/velocity_status` | `autoware_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | +| `/control/turn_signal_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/localization/acceleration` | `geometry_msgs::msg::AccelWithCovarianceStamped` | The topic is the acceleration | ## Parameter diff --git a/common/tier4_vehicle_rviz_plugin/package.xml b/common/tier4_vehicle_rviz_plugin/package.xml index f6c131fdc99f3..922ebd2c5e318 100644 --- a/common/tier4_vehicle_rviz_plugin/package.xml +++ b/common/tier4_vehicle_rviz_plugin/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs libqt5-core libqt5-gui libqt5-widgets diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp index 5d3684d0351c6..5a9f7e602efc7 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -164,7 +164,7 @@ void ConsoleMeterDisplay::update(float wall_dt, float ros_dt) } void ConsoleMeterDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index cba0fa16b50fe..8f1a62fc35361 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -28,13 +28,13 @@ #include #include -#include +#include #endif namespace rviz_plugins { class ConsoleMeterDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -52,7 +52,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; rviz_common::properties::IntProperty * property_left_; @@ -86,7 +86,7 @@ private Q_SLOTS: Arc outer_arc_; std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp index 60a81e45c9c29..ffc2484c3673a 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp @@ -167,7 +167,7 @@ void SteeringAngleDisplay::update(float wall_dt, float ros_dt) } void SteeringAngleDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp index d56b5a8d742b9..867a4c6110bb0 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp @@ -26,13 +26,13 @@ #include #include -#include +#include #endif namespace rviz_plugins { class SteeringAngleDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -50,7 +50,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; @@ -65,7 +65,7 @@ private Q_SLOTS: private: std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp index b9ff54ef44ecd..ec16b693a5974 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp @@ -84,7 +84,7 @@ void TurnSignalDisplay::onDisable() } void TurnSignalDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; @@ -123,19 +123,19 @@ void TurnSignalDisplay::update(float wall_dt, float ros_dt) // turn signal color QColor white_color(Qt::white); white_color.setAlpha(255); - if (signal_type == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) { + if (signal_type == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) { painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); painter.drawPolygon(left_arrow_polygon_, 7); painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(right_arrow_polygon_, 7); - } else if (signal_type == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT) { + } else if (signal_type == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT) { painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); painter.drawPolygon(right_arrow_polygon_, 7); painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(left_arrow_polygon_, 7); - } else if (signal_type == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE) { + } else if (signal_type == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE) { painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(right_arrow_polygon_, 7); diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp index 06a1d2e5f0d54..eb64edd2d595e 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp @@ -24,14 +24,14 @@ #include #include -#include -#include +#include +#include #endif namespace rviz_plugins { class TurnSignalDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -49,7 +49,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::IntProperty * property_left_; rviz_common::properties::IntProperty * property_top_; @@ -62,7 +62,7 @@ private Q_SLOTS: QPointF left_arrow_polygon_[7]; std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp index b5db539f437cb..ccf84d4e64895 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp @@ -102,7 +102,7 @@ void VelocityHistoryDisplay::reset() } bool VelocityHistoryDisplay::validateFloats( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr) { if (!rviz_common::validateFloats(msg_ptr->longitudinal_velocity)) { return false; @@ -120,7 +120,7 @@ void VelocityHistoryDisplay::update(float wall_dt, float ros_dt) } void VelocityHistoryDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp index 9b5819df98bef..96345205289e6 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -36,7 +36,7 @@ namespace rviz_plugins { class VelocityHistoryDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -53,7 +53,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; std::unique_ptr setColorDependsOnVelocity( const double vel_max, const double cmd_vel); std::unique_ptr gradation( @@ -67,11 +67,9 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * property_vel_max_; private: - std::deque< - std::tuple> + std::deque> histories_; - bool validateFloats( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr); + bool validateFloats(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr); std::mutex mutex_; }; diff --git a/common/traffic_light_recognition_marker_publisher/Readme.md b/common/traffic_light_recognition_marker_publisher/Readme.md index 1cafc619ff6fb..d87fc33aefd33 100644 --- a/common/traffic_light_recognition_marker_publisher/Readme.md +++ b/common/traffic_light_recognition_marker_publisher/Readme.md @@ -1,4 +1,4 @@ -# path_distance_calculator +# Traffic Light Recognition Marker Publisher ## Purpose @@ -12,10 +12,10 @@ This node publishes a marker array for visualizing traffic signal recognition re ### Input -| Name | Type | Description | -| ------------------------------------------------------- | -------------------------------------------------------- | ------------------------------------------------- | -| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Vector map for getting traffic signal information | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | The result of traffic signal recognition | +| Name | Type | Description | +| ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------------------- | +| `/map/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | Vector map for getting traffic signal information | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | The result of traffic signal recognition | ### Output diff --git a/common/traffic_light_recognition_marker_publisher/package.xml b/common/traffic_light_recognition_marker_publisher/package.xml index d12022c01684e..2b65c5b868313 100644 --- a/common/traffic_light_recognition_marker_publisher/package.xml +++ b/common/traffic_light_recognition_marker_publisher/package.xml @@ -12,8 +12,8 @@ ament_cmake autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_perception_msgs + autoware_map_msgs + autoware_perception_msgs geometry_msgs lanelet2_extension rclcpp diff --git a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp index 6076d3fa32f26..5d887a2296137 100644 --- a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp +++ b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp @@ -26,17 +26,17 @@ TrafficLightRecognitionMarkerPublisher::TrafficLightRecognitionMarkerPublisher( { using std::placeholders::_1; - sub_map_ptr_ = this->create_subscription( + sub_map_ptr_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS{1}.transient_local(), std::bind(&TrafficLightRecognitionMarkerPublisher::onMap, this, _1)); - sub_tlr_ptr_ = this->create_subscription( + sub_tlr_ptr_ = this->create_subscription( "~/input/traffic_signals", rclcpp::QoS{1}, std::bind(&TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray, this, _1)); pub_marker_ptr_ = this->create_publisher("~/output/marker", rclcpp::QoS{1}); } -void TrafficLightRecognitionMarkerPublisher::onMap(const HADMapBin::ConstSharedPtr msg_ptr) +void TrafficLightRecognitionMarkerPublisher::onMap(const LaneletMapBin::ConstSharedPtr msg_ptr) { is_map_ready_ = false; lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); @@ -70,10 +70,10 @@ void TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray( } MarkerArray markers; marker_id = 0; - for (const auto & tl : msg_ptr->signals) { - if (tl_position_map_.count(tl.map_primitive_id) == 0) continue; - auto tl_position = tl_position_map_[tl.map_primitive_id]; - for (const auto tl_light : tl.lights) { + for (const auto & tl : msg_ptr->traffic_light_groups) { + if (tl_position_map_.count(tl.traffic_light_group_id) == 0) continue; + auto tl_position = tl_position_map_[tl.traffic_light_group_id]; + for (const auto tl_light : tl.elements) { const auto marker = getTrafficLightMarker(tl_position, tl_light.color, tl_light.shape); markers.markers.emplace_back(marker); marker_id++; diff --git a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp index de9aea14b7e5a..c1cac302647a1 100644 --- a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp +++ b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -32,21 +32,21 @@ class TrafficLightRecognitionMarkerPublisher : public rclcpp::Node { public: - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; - using TrafficSignalArray = autoware_auto_perception_msgs::msg::TrafficSignalArray; - using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; + using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; + using TrafficLight = autoware_perception_msgs::msg::TrafficLightElement; using MarkerArray = visualization_msgs::msg::MarkerArray; using Pose = geometry_msgs::msg::Pose; explicit TrafficLightRecognitionMarkerPublisher(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_map_ptr_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_map_ptr_; + rclcpp::Subscription::SharedPtr sub_tlr_ptr_; rclcpp::Publisher::SharedPtr pub_marker_ptr_; - void onMap(const HADMapBin::ConstSharedPtr msg_ptr); + void onMap(const LaneletMapBin::ConstSharedPtr msg_ptr); void onTrafficSignalArray(const TrafficSignalArray::ConstSharedPtr msg_ptr); visualization_msgs::msg::Marker getTrafficLightMarker( const Pose & tl_pose, const uint8_t tl_color, const uint8_t tl_shape); diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index 9c3acd4f45fa4..cf4ecdd9774b8 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -15,8 +15,8 @@ #ifndef TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ #define TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ -#include "autoware_perception_msgs/msg/traffic_signal.hpp" -#include "autoware_perception_msgs/msg/traffic_signal_element.hpp" +#include "autoware_perception_msgs/msg/traffic_light_element.hpp" +#include "autoware_perception_msgs/msg/traffic_light_group.hpp" #include "tier4_perception_msgs/msg/traffic_light.hpp" #include "tier4_perception_msgs/msg/traffic_light_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" @@ -50,7 +50,7 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float c * @return True if a circle-shaped light with the specified color is found, false otherwise. */ bool hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_color); /** * @brief Checks if a traffic light state includes a light with the specified shape. @@ -62,7 +62,7 @@ bool hasTrafficLightCircleColor( * @return True if a light with the specified shape is found, false otherwise. */ bool hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_shape); /** * @brief Determines if a traffic signal indicates a stop for the given lanelet. @@ -78,7 +78,7 @@ bool hasTrafficLightShape( */ bool isTrafficSignalStop( const lanelet::ConstLanelet & lanelet, - const autoware_perception_msgs::msg::TrafficSignal & tl_state); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state); tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light); diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index 0bd3d85a9b71f..8aea884510ec2 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -52,11 +52,11 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float c } bool hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_color) { const auto it_lamp = std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE && + return x.shape == autoware_perception_msgs::msg::TrafficLightElement::CIRCLE && x.color == lamp_color; }); @@ -64,7 +64,7 @@ bool hasTrafficLightCircleColor( } bool hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_shape) { const auto it_lamp = std::find_if( tl_state.elements.begin(), tl_state.elements.end(), @@ -75,10 +75,10 @@ bool hasTrafficLightShape( bool isTrafficSignalStop( const lanelet::ConstLanelet & lanelet, - const autoware_perception_msgs::msg::TrafficSignal & tl_state) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state) { if (hasTrafficLightCircleColor( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::GREEN)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::GREEN)) { return false; } @@ -90,18 +90,18 @@ bool isTrafficSignalStop( if ( turn_direction == "right" && hasTrafficLightShape( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW)) { return false; } if ( turn_direction == "left" && hasTrafficLightShape( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::LEFT_ARROW)) { return false; } if ( turn_direction == "straight" && - hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW)) { + hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficLightElement::UP_ARROW)) { return false; } diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md index 7a7bf212320e0..23f4476d9659d 100644 --- a/control/autonomous_emergency_braking/README.md +++ b/control/autonomous_emergency_braking/README.md @@ -16,6 +16,24 @@ This module has following assumptions. ![aeb_range](./image/range.drawio.svg) +### IMU path generation: steering angle vs IMU's angular velocity + +Currently, the IMU-based path is generated using the angular velocity obtained by the IMU itself. It has been suggested that the steering angle could be used instead onf the angular velocity. + +The pros and cons of both approaches are: + +IMU angular velocity: + +- (+) Usually, it has high accuracy +- (-)Vehicle vibration might introduce noise. + +Steering angle: + +- (+) Not so noisy +- (-) May have a steering offset or a wrong gear ratio, and the steering angle of Autoware and the real steering may not be the same. + +For the moment, there are no plans to implement the steering angle on the path creation process of the AEB module. + ### Limitations - AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud. diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index 27f04603d6a31..81ab2ecff790b 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -52,9 +52,9 @@ namespace autoware::motion::control::autonomous_emergency_braking { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_system_msgs::msg::AutowareState; -using autoware_auto_vehicle_msgs::msg::VelocityReport; +using autoware_planning_msgs::msg::Trajectory; +using autoware_system_msgs::msg::AutowareState; +using autoware_vehicle_msgs::msg::VelocityReport; using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 68c070a86dd97..bf27e6b7e1575 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -14,9 +14,10 @@ ament_cmake autoware_cmake - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs motion_utils diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 905b66df288b4..71eb92a4e95fb 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -301,10 +301,13 @@ bool AEB::fetchLatestData() } const auto imu_ptr = sub_imu_.takeData(); - if (use_imu_path_ && !imu_ptr) { - return missing("imu message"); + if (use_imu_path_) { + if (!imu_ptr) { + return missing("imu message"); + } + // imu_ptr is valid + onImu(imu_ptr); } - onImu(imu_ptr); if (use_imu_path_ && !angular_velocity_ptr_) { return missing("imu"); } diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index 1fa87c2caf3a9..3895b3cc13465 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -25,13 +25,13 @@ Error acceleration calculations are made based on the velocity calculations abov ### Input topics -| Name | Type | Description | -| ---------------------------------------- | -------------------------------------------------------- | ------------------------------------------- | -| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Output trajectory from planning module. | -| `/control/command/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | Output control command from control module. | -| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | -| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | -| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | +| Name | Type | Description | +| ---------------------------------------- | ------------------------------------------ | ------------------------------------------- | +| `/planning/scenario_planning/trajectory` | autoware_planning_msgs::msg::Trajectory | Output trajectory from planning module. | +| `/control/command/control_cmd` | autoware_control_msgs::msg::Control | Output control command from control module. | +| `/vehicle/status/steering_status` | autoware_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | +| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | +| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | ### Output topics diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp index 6bf8fb1c34f5e..6ecf453bf7ab3 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp @@ -24,9 +24,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -39,9 +39,9 @@ namespace control_performance_analysis { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Control; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::Error; using control_performance_analysis::msg::ErrorStamped; @@ -73,7 +73,7 @@ class ControlPerformanceAnalysisCore // Setters void setCurrentPose(const Pose & msg); void setCurrentWaypoints(const Trajectory & trajectory); - void setCurrentControlValue(const AckermannControlCommand & msg); + void setCurrentControlValue(const Control & msg); void setInterpolatedVars( const Pose & interpolated_pose, const double & interpolated_velocity, const double & interpolated_acceleration, const double & interpolated_steering_angle); @@ -100,10 +100,10 @@ class ControlPerformanceAnalysisCore Params p_; // Variables Received Outside - std::shared_ptr current_trajectory_ptr_; + std::shared_ptr current_trajectory_ptr_; std::shared_ptr current_vec_pose_ptr_; std::shared_ptr> odom_history_ptr_; // velocities at k-2, k-1, k, k+1 - std::shared_ptr current_control_ptr_; + std::shared_ptr current_control_ptr_; std::shared_ptr current_vec_steering_msg_ptr_; // State holder diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index 2d5ab8cce002d..73d48db6400a5 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -36,9 +36,9 @@ namespace control_performance_analysis { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Control; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::ErrorStamped; using geometry_msgs::msg::PoseStamped; @@ -52,9 +52,8 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node private: // Subscribers and Local Variable Assignment rclcpp::Subscription::SharedPtr sub_trajectory_; // subscribe to trajectory - rclcpp::Subscription::SharedPtr - sub_control_cmd_; // subscribe to steering control value - rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity + rclcpp::Subscription::SharedPtr sub_control_cmd_; // subscribe to steering control value + rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity rclcpp::Subscription::SharedPtr sub_vehicle_steering_; // Publishers @@ -68,26 +67,26 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node // Callback Methods void onTrajectory(const Trajectory::ConstSharedPtr msg); - void onControlRaw(const AckermannControlCommand::ConstSharedPtr control_msg); + void onControlRaw(const Control::ConstSharedPtr control_msg); void onVecSteeringMeasured(const SteeringReport::ConstSharedPtr meas_steer_msg); void onVelocity(const Odometry::ConstSharedPtr msg); // Parameters Params param_{}; // wheelbase, control period and feedback coefficients. // State holder - AckermannControlCommand::ConstSharedPtr last_control_cmd_; + Control::ConstSharedPtr last_control_cmd_; double d_control_cmd_{0}; // Subscriber Parameters Trajectory::ConstSharedPtr current_trajectory_ptr_; // ConstPtr to local traj. - AckermannControlCommand::ConstSharedPtr current_control_msg_ptr_; + Control::ConstSharedPtr current_control_msg_ptr_; SteeringReport::ConstSharedPtr current_vec_steering_msg_ptr_; Odometry::ConstSharedPtr current_odom_ptr_; PoseStamped::ConstSharedPtr current_pose_; // pose of the vehicle, x, y, heading // prev states Trajectory prev_traj; - AckermannControlCommand prev_cmd; + Control prev_cmd; SteeringReport prev_steering; // Algorithm diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 7003b7d931fa8..a26f4164a177b 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -24,9 +24,9 @@ builtin_interfaces - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs geometry_msgs libboost-dev motion_utils diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index f4ed125956bfa..51c818205e047 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -75,9 +75,9 @@ void ControlPerformanceAnalysisCore::setCurrentPose(const Pose & msg) current_vec_pose_ptr_ = std::make_shared(msg); } -void ControlPerformanceAnalysisCore::setCurrentControlValue(const AckermannControlCommand & msg) +void ControlPerformanceAnalysisCore::setCurrentControlValue(const Control & msg) { - current_control_ptr_ = std::make_shared(msg); + current_control_ptr_ = std::make_shared(msg); } std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPointIdx_path_direction() diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index 768aed321b4f0..693194e8b70a6 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -24,7 +24,7 @@ namespace { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::ErrorStamped; @@ -62,7 +62,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( "~/input/reference_trajectory", 1, std::bind(&ControlPerformanceAnalysisNode::onTrajectory, this, _1)); - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control_raw", 1, std::bind(&ControlPerformanceAnalysisNode::onControlRaw, this, _1)); sub_vehicle_steering_ = create_subscription( @@ -93,8 +93,7 @@ void ControlPerformanceAnalysisNode::onTrajectory(const Trajectory::ConstSharedP current_trajectory_ptr_ = msg; } -void ControlPerformanceAnalysisNode::onControlRaw( - const AckermannControlCommand::ConstSharedPtr control_msg) +void ControlPerformanceAnalysisNode::onControlRaw(const Control::ConstSharedPtr control_msg) { if (last_control_cmd_) { const rclcpp::Duration & duration = diff --git a/control/control_validator/README.md b/control/control_validator/README.md index 3d78721a0a040..9c4a9be0732a5 100644 --- a/control/control_validator/README.md +++ b/control/control_validator/README.md @@ -20,11 +20,11 @@ Other features are to be implemented. The `control_validator` takes in the following inputs: -| Name | Type | Description | -| ------------------------------ | ------------------------------------- | ------------------------------------------------------------------------------ | -| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | -| `~/input/reference_trajectory` | autoware_auto_control_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed | -| `~/input/predicted_trajectory` | autoware_auto_control_msgs/Trajectory | predicted trajectory which is outputted from control module | +| Name | Type | Description | +| ------------------------------ | --------------------------------- | ------------------------------------------------------------------------------ | +| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | +| `~/input/reference_trajectory` | autoware_planning_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed | +| `~/input/predicted_trajectory` | autoware_planning_msgs/Trajectory | predicted trajectory which is outputted from control module | ### Outputs diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp index eba9bf700ba08..e048ef03bf11a 100644 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -31,8 +31,8 @@ namespace control_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using control_validator::msg::ControlValidatorStatus; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; diff --git a/control/control_validator/include/control_validator/debug_marker.hpp b/control/control_validator/include/control_validator/debug_marker.hpp index 794912e085949..2d3a209cbd7da 100644 --- a/control/control_validator/include/control_validator/debug_marker.hpp +++ b/control/control_validator/include/control_validator/debug_marker.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -32,8 +32,7 @@ class ControlValidatorDebugMarkerPublisher explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node); void pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, - int id = 0); + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0); void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); void pushVirtualWall(const geometry_msgs::msg::Pose & pose); void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); diff --git a/control/control_validator/include/control_validator/utils.hpp b/control/control_validator/include/control_validator/utils.hpp index 8033ac9442960..edf97aaf5f510 100644 --- a/control/control_validator/include/control_validator/utils.hpp +++ b/control/control_validator/include/control_validator/utils.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -26,8 +26,8 @@ namespace control_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; using motion_utils::convertToTrajectory; using motion_utils::convertToTrajectoryPointArray; diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml index faf254708ddbb..2f4400d6ebc57 100644 --- a/control/control_validator/package.xml +++ b/control/control_validator/package.xml @@ -20,7 +20,7 @@ autoware_cmake rosidl_default_generators - autoware_auto_planning_msgs + autoware_planning_msgs diagnostic_updater geometry_msgs motion_utils diff --git a/control/control_validator/src/debug_marker.cpp b/control/control_validator/src/debug_marker.cpp index 1b92aee0bb416..2dbdc558b305b 100644 --- a/control/control_validator/src/debug_marker.cpp +++ b/control/control_validator/src/debug_marker.cpp @@ -40,7 +40,7 @@ void ControlValidatorDebugMarkerPublisher::clearMarkers() } void ControlValidatorDebugMarkerPublisher::pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) { pushPoseMarker(p.pose, ns, id); } diff --git a/control/external_cmd_selector/README.md b/control/external_cmd_selector/README.md index 93ded5713a25b..ca0f4f0954dbf 100644 --- a/control/external_cmd_selector/README.md +++ b/control/external_cmd_selector/README.md @@ -23,12 +23,12 @@ The current mode is set via service, `remote` is remotely operated, `local` is t ### Output topics -| Name | Type | Description | -| ------------------------------------------------------ | ------------------------------------------------------ | ----------------------------------------------- | -| `/control/external_cmd_selector/current_selector_mode` | TBD | Current selected mode, remote or local. | -| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Check if node is active or not. | -| `/external/selected/external_control_cmd` | TBD | Pass through control command with current mode. | -| `/external/selected/gear_cmd` | autoware_auto_vehicle_msgs::msg::GearCommand | Pass through gear command with current mode. | -| `/external/selected/hazard_lights_cmd` | autoware_auto_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | -| `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | -| `/external/selected/turn_indicators_cmd` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | +| Name | Type | Description | +| ------------------------------------------------------ | ------------------------------------------------- | ----------------------------------------------- | +| `/control/external_cmd_selector/current_selector_mode` | TBD | Current selected mode, remote or local. | +| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Check if node is active or not. | +| `/external/selected/external_control_cmd` | TBD | Pass through control command with current mode. | +| `/external/selected/gear_cmd` | autoware_vehicle_msgs::msg::GearCommand | Pass through gear command with current mode. | +| `/external/selected/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | +| `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | +| `/external/selected/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | diff --git a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp b/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp index f2803e897ba3e..6a4fb897b57bc 100644 --- a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp +++ b/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp @@ -19,9 +19,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -39,9 +39,9 @@ class ExternalCmdSelector : public rclcpp::Node private: using CommandSourceSelect = tier4_control_msgs::srv::ExternalCommandSelect; using CommandSourceMode = tier4_control_msgs::msg::ExternalCommandSelectorMode; - using InternalGearShift = autoware_auto_vehicle_msgs::msg::GearCommand; - using InternalTurnSignal = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; - using InternalHazardSignal = autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using InternalGearShift = autoware_vehicle_msgs::msg::GearCommand; + using InternalTurnSignal = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + using InternalHazardSignal = autoware_vehicle_msgs::msg::HazardLightsCommand; using InternalHeartbeat = tier4_external_api_msgs::msg::Heartbeat; using ExternalControlCommand = tier4_external_api_msgs::msg::ControlCommandStamped; using ExternalGearShift = tier4_external_api_msgs::msg::GearShiftStamped; diff --git a/control/external_cmd_selector/package.xml b/control/external_cmd_selector/package.xml index 4d8cc5a385494..b3520839e3b51 100644 --- a/control/external_cmd_selector/package.xml +++ b/control/external_cmd_selector/package.xml @@ -16,7 +16,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs rclcpp diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index 4674ebdadb51d..73e3644df49ac 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -30,15 +30,15 @@ ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/ ### Output topics -| Name | Type | Description | -| ----------------------------------- | -------------------------------------------------------- | ---------------------------------------- | -| `~/output/control_command` | autoware_auto_control_msgs::msg::AckermannControlCommand | lateral and longitudinal control command | -| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | -| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command | -| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command | -| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) | -| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat | -| `~/output/vehicle_engage` | autoware_auto_vehicle_msgs::msg::Engage | vehicle engage | +| Name | Type | Description | +| ----------------------------------- | --------------------------------------------------- | ---------------------------------------- | +| `~/output/control_command` | autoware_control_msgs::msg::Control | lateral and longitudinal control command | +| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | +| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command | +| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command | +| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) | +| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat | +| `~/output/vehicle_engage` | autoware_vehicle_msgs::msg::Engage | vehicle engage | ## Parameters diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp index 22064f9cefaad..e88f7bcb3904e 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/joy_controller/include/joy_controller/joy_controller.hpp @@ -19,8 +19,8 @@ #include -#include -#include +#include +#include #include #include #include @@ -81,15 +81,14 @@ class AutowareJoyControllerNode : public rclcpp::Node void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); // Publisher - rclcpp::Publisher::SharedPtr - pub_control_command_; + rclcpp::Publisher::SharedPtr pub_control_command_; rclcpp::Publisher::SharedPtr pub_external_control_command_; rclcpp::Publisher::SharedPtr pub_shift_; rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_heartbeat_; rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_vehicle_engage_; + rclcpp::Publisher::SharedPtr pub_vehicle_engage_; void publishControlCommand(); void publishExternalControlCommand(); @@ -106,7 +105,7 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::Client::SharedPtr client_autoware_engage_; // Previous State - autoware_auto_control_msgs::msg::AckermannControlCommand prev_control_command_; + autoware_control_msgs::msg::Control prev_control_command_; tier4_external_api_msgs::msg::ControlCommand prev_external_control_command_; GearShiftType prev_shift_ = tier4_external_api_msgs::msg::GearShift::NONE; TurnSignalType prev_turn_signal_ = tier4_external_api_msgs::msg::TurnSignal::NONE; diff --git a/control/joy_controller/package.xml b/control/joy_controller/package.xml index 79ee9f27868f2..f7a5ed805b8ad 100644 --- a/control/joy_controller/package.xml +++ b/control/joy_controller/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs geometry_msgs joy nav_msgs diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 5eec438032410..39198825f9c4e 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -254,7 +254,7 @@ void AutowareJoyControllerNode::onTimer() void AutowareJoyControllerNode::publishControlCommand() { - autoware_auto_control_msgs::msg::AckermannControlCommand cmd; + autoware_control_msgs::msg::Control cmd; cmd.stamp = this->now(); { cmd.lateral.steering_tire_angle = steer_ratio_ * joy_->steer(); @@ -262,24 +262,24 @@ void AutowareJoyControllerNode::publishControlCommand() if (joy_->accel()) { cmd.longitudinal.acceleration = accel_ratio_ * joy_->accel(); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = twist_->twist.linear.x + velocity_gain_ * cmd.longitudinal.acceleration; - cmd.longitudinal.speed = - std::min(cmd.longitudinal.speed, static_cast(max_forward_velocity_)); + cmd.longitudinal.velocity = + std::min(cmd.longitudinal.velocity, static_cast(max_forward_velocity_)); } if (joy_->brake()) { - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = -brake_ratio_ * joy_->brake(); } // Backward if (joy_->accel() && joy_->brake()) { cmd.longitudinal.acceleration = backward_accel_ratio_ * joy_->accel(); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = twist_->twist.linear.x - velocity_gain_ * cmd.longitudinal.acceleration; - cmd.longitudinal.speed = - std::max(cmd.longitudinal.speed, static_cast(-max_backward_velocity_)); + cmd.longitudinal.velocity = + std::max(cmd.longitudinal.velocity, static_cast(-max_backward_velocity_)); } } @@ -426,7 +426,7 @@ void AutowareJoyControllerNode::publishAutowareEngage() void AutowareJoyControllerNode::publishVehicleEngage() { - autoware_auto_vehicle_msgs::msg::Engage engage; + autoware_vehicle_msgs::msg::Engage engage; if (joy_->vehicle_engage()) { engage.engage = true; @@ -492,8 +492,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & // Publisher pub_control_command_ = - this->create_publisher( - "output/control_command", 1); + this->create_publisher("output/control_command", 1); pub_external_control_command_ = this->create_publisher( "output/external_control_command", 1); @@ -505,7 +504,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & pub_heartbeat_ = this->create_publisher("output/heartbeat", 1); pub_vehicle_engage_ = - this->create_publisher("output/vehicle_engage", 1); + this->create_publisher("output/vehicle_engage", 1); // Service Client client_emergency_stop_ = this->create_client( diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 67b770aefb3d2..6eb62dcb2c23a 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -49,10 +49,10 @@ This package includes the following features: ### Input - /localization/kinematic_state [`nav_msgs::msg::Odometry`] -- /map/vector_map [`autoware_auto_mapping_msgs::msg::HADMapBin`] +- /map/vector_map [`autoware_map_msgs::msg::LaneletMapBin`] - /planning/mission_planning/route [`autoware_planning_msgs::msg::LaneletRoute`] -- /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs::msg::Trajectory`] -- /control/trajectory_follower/predicted_trajectory [`autoware_auto_planning_msgs::msg::Trajectory`] +- /planning/scenario_planning/trajectory [`autoware_planning_msgs::msg::Trajectory`] +- /control/trajectory_follower/predicted_trajectory [`autoware_planning_msgs::msg::Trajectory`] ### Output @@ -93,6 +93,7 @@ This package includes the following features: | Name | Type | Description | Default value | | :------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ | | footprint_margin_scale | double | Coefficient for expanding footprint margin. Multiplied by 1 standard deviation. | 1.0 | +| footprint_extra_margin | double | Coefficient for expanding footprint margin. When checking for lane departure | 0.0 | | resample_interval | double | Minimum Euclidean distance between points when resample trajectory.[m] | 0.3 | | max_deceleration | double | Maximum deceleration when calculating braking distance. | 2.8 | | delay_time | double | Delay time which took to actuate brake when calculating braking distance. [second] | 1.3 | diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index f0a8df21c425b..2724c28e2536a 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -17,6 +17,7 @@ # Core footprint_margin_scale: 1.0 + footprint_extra_margin: 0.0 resample_interval: 0.3 max_deceleration: 2.8 delay_time: 1.3 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index c658cf4497973..967cb65c8efa8 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -20,14 +20,14 @@ #include #include -#include -#include -#include #include +#include +#include #include #include #include #include +#include #include #include @@ -47,19 +47,20 @@ namespace lane_departure_checker { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::PoseDeviation; using tier4_autoware_utils::Segment2d; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; struct Param { double footprint_margin_scale; + double footprint_extra_margin; double resample_interval; double max_deceleration; double delay_time; diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index dd05ab226f6b7..0f0e15d0a4f62 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -22,9 +22,9 @@ #include #include -#include -#include +#include #include +#include #include #include #include @@ -42,7 +42,7 @@ namespace lane_departure_checker { -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; struct NodeParam { @@ -67,7 +67,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; @@ -87,7 +87,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node // Callback void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onLaneletMapBin(const HADMapBin::ConstSharedPtr msg); + void onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg); void onRoute(const LaneletRoute::ConstSharedPtr msg); void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index 1059e86cadc6d..73955613d21a9 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -11,8 +11,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_planning_msgs boost diagnostic_updater diff --git a/control/lane_departure_checker/schema/lane_departure_checker.json b/control/lane_departure_checker/schema/lane_departure_checker.json index c7f39fbf7ef8a..26e2f92f78ab4 100644 --- a/control/lane_departure_checker/schema/lane_departure_checker.json +++ b/control/lane_departure_checker/schema/lane_departure_checker.json @@ -11,6 +11,11 @@ "default": 1.0, "description": "Coefficient for expanding footprint margin. Multiplied by 1 standard deviation." }, + "footprint_extra_margin": { + "type": "number", + "default": 10.0, + "description": "Coefficient for expanding footprint margin." + }, "resample_interval": { "type": "number", "default": 0.3, @@ -53,6 +58,7 @@ }, "required": [ "footprint_margin_scale", + "footprint_extra_margin", "resample_interval", "max_deceleration", "max_lateral_deviation", 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 c48383a17ab4b..eb5d674705aaa 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 @@ -39,8 +39,8 @@ using tier4_autoware_utils::Point2d; namespace { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Point; @@ -261,7 +261,8 @@ std::vector LaneDepartureChecker::createVehicleFootprints( const PathWithLaneId & path) const { // Create vehicle footprint in base_link coordinate - const auto local_vehicle_footprint = vehicle_info_ptr_->createFootprint(); + const auto local_vehicle_footprint = + vehicle_info_ptr_->createFootprint(param_.footprint_extra_margin); // Create vehicle footprint on each Path point std::vector vehicle_footprints; 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 06d11133920f4..c3fd1d314e371 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 @@ -150,6 +150,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o // Core Parameter param_.footprint_margin_scale = declare_parameter("footprint_margin_scale"); + param_.footprint_extra_margin = declare_parameter("footprint_extra_margin"); param_.resample_interval = declare_parameter("resample_interval"); param_.max_deceleration = declare_parameter("max_deceleration"); param_.delay_time = declare_parameter("delay_time"); @@ -171,7 +172,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o // Subscriber sub_odom_ = this->create_subscription( "~/input/odometry", 1, std::bind(&LaneDepartureCheckerNode::onOdometry, this, _1)); - sub_lanelet_map_bin_ = this->create_subscription( + sub_lanelet_map_bin_ = this->create_subscription( "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(), std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1)); sub_route_ = this->create_subscription( @@ -205,7 +206,7 @@ void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSh current_odom_ = msg; } -void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr msg) +void LaneDepartureCheckerNode::onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_); @@ -403,6 +404,7 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( // Core update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); + update_param(parameters, "footprint_extra_margin", param_.footprint_extra_margin); update_param(parameters, "resample_interval", param_.resample_interval); update_param(parameters, "max_deceleration", param_.max_deceleration); update_param(parameters, "delay_time", param_.delay_time); diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 6bfba5818bd06..3ad116929b50c 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -63,15 +63,15 @@ The tracking is not accurate if the first point of the reference trajectory is a Set the following from the [controller_node](../trajectory_follower_node/README.md) -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport`: current steering +- `autoware_vehicle_msgs/SteeringReport`: current steering ### Outputs Return LateralOutput which contains the following to the controller node -- `autoware_auto_control_msgs/AckermannLateralCommand` +- `autoware_control_msgs/Lateral` - LateralSyncData - steer angle convergence diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 2e83c5ab847c4..91b803dea36fa 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -22,9 +22,9 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" @@ -38,9 +38,9 @@ namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; @@ -378,7 +378,7 @@ class MPC */ Float32MultiArrayStamped generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, - const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, + const MPCMatrix & mpc_matrix, const Lateral & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const; /** @@ -441,9 +441,8 @@ class MPC * @return True if the MPC calculation is successful, false otherwise. */ bool calculateMPC( - const SteeringReport & current_steer, const Odometry & current_kinematics, - AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, - Float32MultiArrayStamped & diagnostic); + const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, + Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic); /** * @brief Set the reference trajectory to be followed. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index 92b01247105c6..eb1d75f9508b3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -22,10 +22,9 @@ #include "rclcpp/rclcpp.hpp" #include "trajectory_follower_base/lateral_controller_base.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -42,9 +41,9 @@ namespace autoware::motion::control::mpc_lateral_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; @@ -97,10 +96,10 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase bool m_is_mpc_history_filled{false}; // store the last mpc outputs for 1 sec - std::vector> m_mpc_steering_history{}; + std::vector> m_mpc_steering_history{}; // set the mpc steering output to history - void setSteeringToHistory(const AckermannLateralCommand & steering); + void setSteeringToHistory(const Lateral & steering); // check if the mpc steering output is converged bool isMpcConverged(); @@ -118,7 +117,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase bool m_is_ctrl_cmd_prev_initialized = false; // Previous control command for path following. - AckermannLateralCommand m_ctrl_cmd_prev; + Lateral m_ctrl_cmd_prev; // Flag indicating whether the first trajectory has been received. bool m_has_received_first_trajectory = false; @@ -200,7 +199,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @param ctrl_cmd Control command to be created. * @return Created control command. */ - AckermannLateralCommand createCtrlCmdMsg(const AckermannLateralCommand & ctrl_cmd); + Lateral createCtrlCmdMsg(const Lateral & ctrl_cmd); /** * @brief Publish the predicted future trajectory. @@ -218,13 +217,13 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @brief Get the stop control command. * @return Stop control command. */ - AckermannLateralCommand getStopControlCommand() const; + Lateral getStopControlCommand() const; /** * @brief Get the control command applied before initialization. * @return Initial control command. */ - AckermannLateralCommand getInitialControlCommand() const; + Lateral getInitialControlCommand() const; /** * @brief Check if the ego car is in a stopped state. @@ -250,7 +249,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @param cmd Steering control command to be checked. * @return True if the steering control is converged and stable, false otherwise. */ - bool isSteerConverged(const AckermannLateralCommand & cmd) const; + bool isSteerConverged(const Lateral & cmd) const; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index 7e1c7ebdf1348..eb624f39ae76b 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -109,11 +109,11 @@ class MPCTrajectory return points; } - std::vector toTrajectoryPoints() const + std::vector toTrajectoryPoints() const { - std::vector points; + std::vector points; for (size_t i = 0; i < x.size(); ++i) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; + autoware_planning_msgs::msg::TrajectoryPoint point; point.pose.position.x = x.at(i); point.pose.position.y = y.at(i); point.pose.position.z = z.at(i); diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 9062ff1ea34e3..819d7fb89b8a7 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -28,8 +28,8 @@ #include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -43,8 +43,8 @@ namespace autoware::motion::control::mpc_lateral_controller namespace MPCUtils { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; /** diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp index 7ca2fa1a61528..16e9b165fb1a5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp @@ -17,14 +17,14 @@ #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include #include namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; +using autoware_control_msgs::msg::Lateral; class SteeringPredictor { @@ -61,7 +61,7 @@ class SteeringPredictor double m_input_delay; // Buffer of sent control commands. - std::vector m_ctrl_cmd_vec; + std::vector m_ctrl_cmd_vec; /** * @brief Get the sum of all steering commands over the given time range. diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml index da03c4481e782..000bddc65aa1f 100644 --- a/control/mpc_lateral_controller/package.xml +++ b/control/mpc_lateral_controller/package.xml @@ -17,9 +17,9 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 373de2e0bd911..177c1e0854bfb 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -36,9 +36,8 @@ MPC::MPC(rclcpp::Node & node) } bool MPC::calculateMPC( - const SteeringReport & current_steer, const Odometry & current_kinematics, - AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, - Float32MultiArrayStamped & diagnostic) + const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, + Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic) { // since the reference trajectory does not take into account the current velocity of the ego // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics. @@ -117,7 +116,7 @@ bool MPC::calculateMPC( Float32MultiArrayStamped MPC::generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, - const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, + const MPCMatrix & mpc_matrix, const Lateral & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const { Float32MultiArrayStamped diagnostic; diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 882150ffc1644..85d33a5e9f1c0 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -239,7 +239,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_current_steering.steering_tire_angle -= steering_offset_->getOffset(); } - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory predicted_traj; Float32MultiArrayStamped debug_values; @@ -309,7 +309,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( return createLateralOutput(ctrl_cmd, is_mpc_solved); } -bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) const +bool MpcLateralController::isSteerConverged(const Lateral & cmd) const { // wait for a while to propagate the trajectory shape to the output command when the trajectory // shape is changed. @@ -381,17 +381,17 @@ void MpcLateralController::setTrajectory( } } -AckermannLateralCommand MpcLateralController::getStopControlCommand() const +Lateral MpcLateralController::getStopControlCommand() const { - AckermannLateralCommand cmd; + Lateral cmd; cmd.steering_tire_angle = static_cast(m_steer_cmd_prev); cmd.steering_tire_rotation_rate = 0.0; return cmd; } -AckermannLateralCommand MpcLateralController::getInitialControlCommand() const +Lateral MpcLateralController::getInitialControlCommand() const { - AckermannLateralCommand cmd; + Lateral cmd; cmd.steering_tire_angle = m_current_steering.steering_tire_angle; cmd.steering_tire_rotation_rate = 0.0; return cmd; @@ -429,8 +429,7 @@ bool MpcLateralController::isStoppedState() const } } -AckermannLateralCommand MpcLateralController::createCtrlCmdMsg( - const AckermannLateralCommand & ctrl_cmd) +Lateral MpcLateralController::createCtrlCmdMsg(const Lateral & ctrl_cmd) { auto out = ctrl_cmd; out.stamp = clock_->now(); @@ -456,7 +455,7 @@ void MpcLateralController::publishDebugValues(Float32MultiArrayStamped & debug_v m_pub_steer_offset->publish(offset); } -void MpcLateralController::setSteeringToHistory(const AckermannLateralCommand & steering) +void MpcLateralController::setSteeringToHistory(const Lateral & steering) { const auto time = clock_->now(); if (m_mpc_steering_history.empty()) { diff --git a/control/mpc_lateral_controller/src/steering_predictor.cpp b/control/mpc_lateral_controller/src/steering_predictor.cpp index f2570047d5bd2..48d8fa8c47a97 100644 --- a/control/mpc_lateral_controller/src/steering_predictor.cpp +++ b/control/mpc_lateral_controller/src/steering_predictor.cpp @@ -47,7 +47,7 @@ double SteeringPredictor::calcSteerPrediction() void SteeringPredictor::storeSteerCmd(const double steer) { const auto time_delayed = m_clock->now() + rclcpp::Duration::from_seconds(m_input_delay); - AckermannLateralCommand cmd; + Lateral cmd; cmd.stamp = time_delayed; cmd.steering_tire_angle = static_cast(steer); diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index ba145b5dd146c..7644fb28b0788 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -21,10 +21,10 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" @@ -41,10 +41,10 @@ namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; @@ -206,7 +206,7 @@ TEST_F(MPCTest, InitializeAndCalculate) initializeMPC(*mpc); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -238,7 +238,7 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -265,7 +265,7 @@ TEST_F(MPCTest, OsqpCalculate) ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -293,7 +293,7 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -323,7 +323,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -354,7 +354,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -379,7 +379,7 @@ TEST_F(MPCTest, DynamicCalculate) ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -403,7 +403,7 @@ TEST_F(MPCTest, MultiSolveWithBuffer) mpc->m_input_buffer = {0.0, 0.0, 0.0}; // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -443,7 +443,7 @@ TEST_F(MPCTest, FailureCases) Pose pose_far; pose_far.position.x = pose_zero.position.x - admissible_position_error - 1.0; pose_far.position.y = pose_zero.position.y - admissible_position_error - 1.0; - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_far, default_velocity); diff --git a/control/mpc_lateral_controller/test/test_mpc_utils.cpp b/control/mpc_lateral_controller/test/test_mpc_utils.cpp index 75a7208074b90..51cc7d55e4560 100644 --- a/control/mpc_lateral_controller/test/test_mpc_utils.cpp +++ b/control/mpc_lateral_controller/test/test_mpc_utils.cpp @@ -16,8 +16,8 @@ #include "mpc_lateral_controller/mpc_trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -25,8 +25,8 @@ namespace { namespace MPCUtils = autoware::motion::control::mpc_lateral_controller::MPCUtils; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; TrajectoryPoint makePoint(const double x, const double y, const float vx) { diff --git a/control/obstacle_collision_checker/README.md b/control/obstacle_collision_checker/README.md index 5478db1f5a135..25b57fe6f4653 100644 --- a/control/obstacle_collision_checker/README.md +++ b/control/obstacle_collision_checker/README.md @@ -57,13 +57,13 @@ If any collision is found on predicted path, this module sets `ERROR` level as d ### Input -| Name | Type | Description | -| ---------------------------------------------- | ---------------------------------------------- | ------------------------------------------------------------------ | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ----------------------------------------- | ------------------------------------------------------------------ | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp index 4ab26c31f9910..a65d818056bd7 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -52,15 +52,15 @@ struct Input geometry_msgs::msg::Twist::ConstSharedPtr current_twist; sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud; geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; }; struct Output { std::map processing_time_map; bool will_collide; - autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory; + autoware_planning_msgs::msg::Trajectory resampled_trajectory; std::vector vehicle_footprints; std::vector vehicle_passing_areas; }; @@ -78,14 +78,14 @@ class ObstacleCollisionChecker vehicle_info_util::VehicleInfo vehicle_info_; //! This function assumes the input trajectory is sampled dense enough - static autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval); + static autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval); - static autoware_auto_planning_msgs::msg::Trajectory cutTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length); + static autoware_planning_msgs::msg::Trajectory cutTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double length); static std::vector createVehicleFootprints( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, const vehicle_info_util::VehicleInfo & vehicle_info); static std::vector createVehiclePassingAreas( diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp index 12b1e51a1cf3c..9d79a0facac95 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include #include @@ -51,9 +51,9 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node std::shared_ptr self_pose_listener_; std::shared_ptr transform_listener_; rclcpp::Subscription::SharedPtr sub_obstacle_pointcloud_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; rclcpp::Subscription::SharedPtr sub_odom_; @@ -62,13 +62,13 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_; geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; // Callback void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); - void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); // Publisher diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index 46b0b18191e81..ef7560755a122 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -18,7 +18,7 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs boost diagnostic_updater eigen diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index 8b89913ef35c8..6887be4cedd77 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -54,7 +54,7 @@ pcl::PointCloud getTransformedPointCloud( pcl::PointCloud filterPointCloudByTrajectory( const pcl::PointCloud & pointcloud, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius) + const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius) { pcl::PointCloud filtered_pointcloud; for (const auto & point : pointcloud.points) { @@ -121,10 +121,10 @@ Output ObstacleCollisionChecker::update(const Input & input) return output; } -autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval) +autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval) { - autoware_auto_planning_msgs::msg::Trajectory resampled; + autoware_planning_msgs::msg::Trajectory resampled; resampled.header = trajectory.header; resampled.points.push_back(trajectory.points.front()); @@ -143,10 +143,10 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleT return resampled; } -autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length) +autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double length) { - autoware_auto_planning_msgs::msg::Trajectory cut; + autoware_planning_msgs::msg::Trajectory cut; cut.header = trajectory.header; double total_length = 0.0; @@ -169,7 +169,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec if (remain_distance <= points_distance) { const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.pose.position.x = p_interpolated.x(); p.pose.position.y = p_interpolated.y(); p.pose.position.z = p_interpolated.z(); @@ -187,7 +187,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec } std::vector ObstacleCollisionChecker::createVehicleFootprints( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, const vehicle_info_util::VehicleInfo & vehicle_info) { // Create vehicle footprint in base_link coordinate diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index bf3da0fe32627..cfde4ee849728 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -76,10 +76,10 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt sub_obstacle_pointcloud_ = create_subscription( "input/obstacle_pointcloud", rclcpp::SensorDataQoS(), std::bind(&ObstacleCollisionCheckerNode::onObstaclePointcloud, this, _1)); - sub_reference_trajectory_ = create_subscription( + sub_reference_trajectory_ = create_subscription( "input/reference_trajectory", 1, std::bind(&ObstacleCollisionCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = create_subscription( + sub_predicted_trajectory_ = create_subscription( "input/predicted_trajectory", 1, std::bind(&ObstacleCollisionCheckerNode::onPredictedTrajectory, this, _1)); sub_odom_ = create_subscription( @@ -109,13 +109,13 @@ void ObstacleCollisionCheckerNode::onObstaclePointcloud( } void ObstacleCollisionCheckerNode::onReferenceTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { reference_trajectory_ = msg; } void ObstacleCollisionCheckerNode::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { predicted_trajectory_ = msg; } diff --git a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp index e851cca85c11f..705bff754d3d9 100644 --- a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp @@ -18,9 +18,9 @@ TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) { pcl::PointCloud pcl; - autoware_auto_planning_msgs::msg::Trajectory trajectory; + autoware_planning_msgs::msg::Trajectory trajectory; pcl::PointXYZ pcl_point; - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + autoware_planning_msgs::msg::TrajectoryPoint traj_point; pcl_point.y = 0.0; traj_point.pose.position.y = 0.99; for (double x = 0.0; x < 10.0; x += 1.0) { diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index ea0688e88d9f1..008d092565ba4 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -58,15 +58,15 @@ For the mode transition: For the transition availability/completion check: -- /control/command/control_cmd [`autoware_auto_control_msgs/msg/AckermannControlCommand`]: vehicle control signal +- /control/command/control_cmd [`autoware_control_msgs/msg/Control`]: vehicle control signal - /localization/kinematic_state [`nav_msgs/msg/Odometry`]: ego vehicle state -- /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs/msg/Trajectory`]: planning trajectory -- /vehicle/status/control_mode [`autoware_auto_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) +- /planning/scenario_planning/trajectory [`autoware_planning_msgs/msg/Trajectory`]: planning trajectory +- /vehicle/status/control_mode [`autoware_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) - /control/vehicle_cmd_gate/operation_mode [`autoware_adapi_v1_msgs/msg/OperationModeState`]: the operation mode in the `vehicle_cmd_gate`. (To be removed) For the backward compatibility (to be removed): -- /api/autoware/get/engage [`autoware_auto_vehicle_msgs/msg/Engage`] +- /api/autoware/get/engage [`autoware_vehicle_msgs/msg/Engage`] - /control/current_gate_mode [`tier4_control_msgs/msg/GateMode`] - /control/external_cmd_selector/current_selector_mode [`tier4_control_msgs/msg/ExternalCommandSelectorMode`] @@ -76,9 +76,9 @@ For the backward compatibility (to be removed): - /control/operation_mode_transition_manager/debug_info [`operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug`]: detailed information about the operation mode transition - /control/gate_mode_cmd [`tier4_control_msgs/msg/GateMode`]: to change the `vehicle_cmd_gate` state to use its features (to be removed) -- /autoware/engage [`autoware_auto_vehicle_msgs/msg/Engage`]: +- /autoware/engage [`autoware_vehicle_msgs/msg/Engage`]: -- /control/control_mode_request [`autoware_auto_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) +- /control/control_mode_request [`autoware_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) - /control/external_cmd_selector/select_external_command [`tier4_control_msgs/srv/ExternalCommandSelect`]: ## Parameters diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index dbe9a21c1186a..99439aa7e3ab7 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -11,9 +11,8 @@ autoware_cmake rosidl_default_generators - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils geometry_msgs diff --git a/control/operation_mode_transition_manager/src/compatibility.hpp b/control/operation_mode_transition_manager/src/compatibility.hpp index a69b15e69960c..d9bde7cb22872 100644 --- a/control/operation_mode_transition_manager/src/compatibility.hpp +++ b/control/operation_mode_transition_manager/src/compatibility.hpp @@ -17,7 +17,7 @@ #include "data.hpp" -#include +#include #include #include #include @@ -33,7 +33,7 @@ class Compatibility std::optional get_mode() const; private: - using AutowareEngage = autoware_auto_vehicle_msgs::msg::Engage; + using AutowareEngage = autoware_vehicle_msgs::msg::Engage; using GateMode = tier4_control_msgs::msg::GateMode; using SelectorModeMsg = tier4_control_msgs::msg::ExternalCommandSelectorMode; using SelectorModeSrv = tier4_control_msgs::srv::ExternalCommandSelect; diff --git a/control/operation_mode_transition_manager/src/data.hpp b/control/operation_mode_transition_manager/src/data.hpp index 9b822936a0252..8ea9f8d2b99ee 100644 --- a/control/operation_mode_transition_manager/src/data.hpp +++ b/control/operation_mode_transition_manager/src/data.hpp @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -34,8 +34,8 @@ using ServiceResponse = autoware_adapi_v1_msgs::srv::ChangeOperationMode::Respon using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; using OperationModeValue = OperationModeState::_mode_type; using ChangeOperationMode = tier4_system_msgs::srv::ChangeOperationMode; -using ControlModeCommand = autoware_auto_vehicle_msgs::srv::ControlModeCommand; -using ControlModeReport = autoware_auto_vehicle_msgs::msg::ControlModeReport; +using ControlModeCommand = autoware_vehicle_msgs::srv::ControlModeCommand; +using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport; enum class OperationMode { STOP, AUTONOMOUS, LOCAL, REMOTE }; diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index d57cb8c78c740..635ead2430677 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -35,13 +35,11 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo(); - sub_control_cmd_ = node->create_subscription( - "control_cmd", 1, - [this](const AckermannControlCommand::SharedPtr msg) { control_cmd_ = *msg; }); - sub_trajectory_follower_control_cmd_ = node->create_subscription( - "trajectory_follower_control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { - trajectory_follower_control_cmd_ = *msg; - }); + sub_control_cmd_ = node->create_subscription( + "control_cmd", 1, [this](const Control::SharedPtr msg) { control_cmd_ = *msg; }); + sub_trajectory_follower_control_cmd_ = node->create_subscription( + "trajectory_follower_control_cmd", 1, + [this](const Control::SharedPtr msg) { trajectory_follower_control_cmd_ = *msg; }); sub_kinematics_ = node->create_subscription( "kinematics", 1, [this](const Odometry::SharedPtr msg) { kinematics_ = *msg; }); @@ -220,7 +218,7 @@ bool AutonomousMode::isModeChangeAvailable() } const auto current_speed = kinematics_.twist.twist.linear.x; - const auto target_control_speed = control_cmd_.longitudinal.speed; + const auto target_control_speed = control_cmd_.longitudinal.velocity; const auto & param = engage_acceptable_param_; if (!enable_engage_on_driving_ && std::fabs(current_speed) > 1.0e-2) { @@ -267,7 +265,7 @@ bool AutonomousMode::isModeChangeAvailable() // No engagement if the vehicle is moving but the target speed is zero. const bool is_stop_cmd_indicated = std::abs(target_control_speed) < 0.01 || - std::abs(trajectory_follower_control_cmd_.longitudinal.speed) < 0.01; + std::abs(trajectory_follower_control_cmd_.longitudinal.velocity) < 0.01; const bool stop_ok = !(std::abs(current_speed) > 0.1 && is_stop_cmd_indicated); // No engagement if the large acceleration is commanded. diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index e10d64e367f8d..e5abd4285ad5f 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -63,11 +63,11 @@ class AutonomousMode : public ModeChangeBase bool hasDangerAcceleration(); std::pair hasDangerLateralAcceleration(); - using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + using Control = autoware_control_msgs::msg::Control; using Odometry = nav_msgs::msg::Odometry; - using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; - rclcpp::Subscription::SharedPtr sub_control_cmd_; - rclcpp::Subscription::SharedPtr sub_trajectory_follower_control_cmd_; + using Trajectory = autoware_planning_msgs::msg::Trajectory; + rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_trajectory_follower_control_cmd_; rclcpp::Subscription::SharedPtr sub_kinematics_; rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Logger logger_; @@ -79,8 +79,8 @@ class AutonomousMode : public ModeChangeBase double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; - AckermannControlCommand control_cmd_; - AckermannControlCommand trajectory_follower_control_cmd_; + Control control_cmd_; + Control trajectory_follower_control_cmd_; Odometry kinematics_; Trajectory trajectory_; vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 57eab2d87f18e..4e20cb27fe66a 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -143,14 +143,14 @@ There are two sources of the slope information, which can be switched by a param Set the following from the [controller_node](../trajectory_follower_node/README.md) -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry ### Output Return LongitudinalOutput which contains the following to the controller node -- `autoware_auto_control_msgs/LongitudinalCommand`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. +- `autoware_control_msgs/Longitudinal`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. - LongitudinalSyncData - velocity convergence(currently not used) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index ac4fec8dacb7d..4ea844a140f4f 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -25,7 +25,7 @@ #include #include // NOLINT -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" #include @@ -37,8 +37,8 @@ namespace autoware::motion::control::pid_longitudinal_controller namespace longitudinal_utils { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Quaternion; @@ -150,7 +150,7 @@ double applyDiffLimitFilter( */ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const size_t src_idx, const double distance, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); + const autoware_planning_msgs::msg::Trajectory & trajectory); } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 967b7c40fbd10..dee2e580e6911 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -33,9 +33,8 @@ #include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -88,7 +87,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro struct ControlData { bool is_far_from_trajectory{false}; - autoware_auto_planning_msgs::msg::Trajectory interpolated_traj{}; + autoware_planning_msgs::msg::Trajectory interpolated_traj{}; size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx size_t target_idx{0}; StateAfterDelay state_after_delay{0.0, 0.0, 0.0}; @@ -113,7 +112,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // pointers for ros topic nav_msgs::msg::Odometry m_current_kinematic_state; geometry_msgs::msg::AccelWithCovarianceStamped m_current_accel; - autoware_auto_planning_msgs::msg::Trajectory m_trajectory; + autoware_planning_msgs::msg::Trajectory m_trajectory; OperationModeState m_current_operation_mode; // vehicle info @@ -218,7 +217,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double m_ego_nearest_yaw_threshold; // buffer of send command - std::vector m_ctrl_cmd_vec; + std::vector m_ctrl_cmd_vec; // for calculating dt std::shared_ptr m_prev_control_time{nullptr}; @@ -270,7 +269,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @brief set reference trajectory with received message * @param [in] msg trajectory message */ - void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg); + void setTrajectory(const autoware_planning_msgs::msg::Trajectory & msg); bool isReady(const trajectory_follower::InputData & input_data) override; @@ -309,7 +308,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] ctrl_cmd calculated control command to control velocity * @param [in] current_vel current velocity of the vehicle */ - autoware_auto_control_msgs::msg::LongitudinalCommand createCtrlCmdMsg( + autoware_control_msgs::msg::Longitudinal createCtrlCmdMsg( const Motion & ctrl_cmd, const double & current_vel); /** @@ -371,9 +370,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] point vehicle position * @param [in] nearest_idx index of the trajectory point nearest to the vehicle position */ - std::pair + std::pair calcInterpolatedTrajPointAndSegment( - const autoware_auto_planning_msgs::msg::Trajectory & traj, + const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const; /** diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml index 959aca689816a..82b297a69fee6 100644 --- a/control/pid_longitudinal_controller/package.xml +++ b/control/pid_longitudinal_controller/package.xml @@ -19,9 +19,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 28cd6e1824859..4e9ef52a4969e 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -161,7 +161,7 @@ double applyDiffLimitFilter( geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const size_t src_idx, const double distance, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) + const autoware_planning_msgs::msg::Trajectory & trajectory) { double remain_dist = distance; geometry_msgs::msg::Pose p = trajectory.points.back().pose; diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index cc1c0c6383707..9a8223c610f9a 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -232,8 +232,7 @@ void PidLongitudinalController::setCurrentOperationMode(const OperationModeState m_current_operation_mode = msg; } -void PidLongitudinalController::setTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & msg) +void PidLongitudinalController::setTrajectory(const autoware_planning_msgs::msg::Trajectory & msg) { if (!longitudinal_utils::isValidTrajectory(msg)) { RCLCPP_ERROR_THROTTLE(logger_, *clock_, 3000, "received invalid trajectory. ignore."); @@ -820,13 +819,13 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( } // Do not use nearest_idx here -autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController::createCtrlCmdMsg( +autoware_control_msgs::msg::Longitudinal PidLongitudinalController::createCtrlCmdMsg( const Motion & ctrl_cmd, const double & current_vel) { // publish control command - autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; + autoware_control_msgs::msg::Longitudinal cmd{}; cmd.stamp = clock_->now(); - cmd.speed = static_cast(ctrl_cmd.vel); + cmd.velocity = static_cast(ctrl_cmd.vel); cmd.acceleration = static_cast(ctrl_cmd.acc); // store current velocity history @@ -926,7 +925,7 @@ void PidLongitudinalController::storeAccelCmd(const double accel) { if (m_control_state == ControlState::DRIVE) { // convert format - autoware_auto_control_msgs::msg::LongitudinalCommand cmd; + autoware_control_msgs::msg::Longitudinal cmd; cmd.stamp = clock_->now(); cmd.acceleration = static_cast(accel); @@ -994,10 +993,9 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop return output_motion; } -std::pair +std::pair PidLongitudinalController::calcInterpolatedTrajPointAndSegment( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & pose) const + const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const { if (traj.points.size() == 1) { return std::make_pair(traj.points.at(0), 0); diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 02bcce8c91c4b..bd79d7a8c3ac3 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -18,8 +18,8 @@ #include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -37,8 +37,8 @@ namespace longitudinal_utils = TEST(TestLongitudinalControllerUtils, isValidTrajectory) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; Trajectory traj; TrajectoryPoint point; EXPECT_FALSE(longitudinal_utils::isValidTrajectory(traj)); @@ -51,8 +51,8 @@ TEST(TestLongitudinalControllerUtils, isValidTrajectory) TEST(TestLongitudinalControllerUtils, calcStopDistance) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; Pose current_pose; current_pose.position.x = 0.0; @@ -115,8 +115,8 @@ TEST(TestLongitudinalControllerUtils, getPitchByPose) TEST(TestLongitudinalControllerUtils, getPitchByTraj) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; const double wheel_base = 0.9; /** * Trajectory: @@ -346,10 +346,10 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; const double abs_err = 1e-15; - decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points; + decltype(autoware_planning_msgs::msg::Trajectory::points) points; TrajectoryPoint p; p.pose.position.x = 0.0; p.pose.position.y = 0.0; @@ -505,8 +505,8 @@ TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter) TEST(TestLongitudinalControllerUtils, findTrajectoryPoseAfterDistance) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; const double abs_err = 1e-5; Trajectory traj; diff --git a/control/predicted_path_checker/README.md b/control/predicted_path_checker/README.md index 24e4b91ef441b..a968a285928f6 100644 --- a/control/predicted_path_checker/README.md +++ b/control/predicted_path_checker/README.md @@ -55,14 +55,14 @@ make the vehicle stop. ## Inputs -| Name | Type | Description | -| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------------- | -| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | -| `~/input/predicted_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | -| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | -| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | -| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | +| Name | Type | Description | +| ------------------------------------- | ------------------------------------------------ | --------------------------------------------------- | +| `~/input/reference_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/predicted_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | +| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | +| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | +| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | ## Outputs diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp index 1d7791955b576..8410e7c78f723 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -42,11 +42,11 @@ namespace autoware::motion::control::predicted_path_checker { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using tier4_autoware_utils::Point2d; diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index 3ce5728521141..23696887c7051 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -27,8 +27,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -47,10 +47,10 @@ namespace autoware::motion::control::predicted_path_checker { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using tier4_autoware_utils::Point2d; @@ -90,9 +90,9 @@ class PredictedPathCheckerNode : public rclcpp::Node // Subscriber std::shared_ptr self_pose_listener_; rclcpp::Subscription::SharedPtr sub_dynamic_objects_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_accel_; @@ -106,8 +106,8 @@ class PredictedPathCheckerNode : public rclcpp::Node geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; control_interface::IsStopped::Message::ConstSharedPtr is_stopped_ptr_{nullptr}; // Core @@ -122,8 +122,8 @@ class PredictedPathCheckerNode : public rclcpp::Node // Callback void onDynamicObjects(PredictedObjects::ConstSharedPtr msg); - void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); - void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); void onIsStopped(const control_interface::IsStopped::Message::ConstSharedPtr msg); diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 75e90e624a17e..957800ad04305 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -39,9 +39,9 @@ namespace utils { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; @@ -85,14 +85,14 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( const double & base_to_width); Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertObjToPolygon(const PredictedObject & obj); -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape); void getCurrentObjectPose( PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index 35f971907696a..bca65302dba55 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -12,8 +12,8 @@ ament_cmake autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs boost component_interface_specs component_interface_utils diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index bba069442bee7..28ea21f639a0e 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -72,10 +72,10 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n sub_dynamic_objects_ = create_subscription( "~/input/objects", rclcpp::SensorDataQoS(), std::bind(&PredictedPathCheckerNode::onDynamicObjects, this, _1)); - sub_reference_trajectory_ = create_subscription( + sub_reference_trajectory_ = create_subscription( "~/input/reference_trajectory", 1, std::bind(&PredictedPathCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = create_subscription( + sub_predicted_trajectory_ = create_subscription( "~/input/predicted_trajectory", 1, std::bind(&PredictedPathCheckerNode::onPredictedTrajectory, this, _1)); sub_odom_ = create_subscription( @@ -109,13 +109,13 @@ void PredictedPathCheckerNode::onDynamicObjects(const PredictedObjects::ConstSha } void PredictedPathCheckerNode::onReferenceTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { reference_trajectory_ = msg; } void PredictedPathCheckerNode::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { predicted_trajectory_ = msg; } diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index 9cb095e908d41..f1c76ce6eef8f 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -298,7 +298,7 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( } Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; @@ -320,7 +320,7 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; tf2::Transform tf_map2obj; @@ -350,15 +350,15 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & Polygon2d convertObjToPolygon(const PredictedObject & obj) { Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = utils::convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { object_polygon = utils::convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); } else { @@ -377,13 +377,13 @@ bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & ob return base_pose_vec.dot(obstacle_vec) >= 0; } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); diff --git a/control/pure_pursuit/README.md b/control/pure_pursuit/README.md index 48e2a13ef664d..55c847fe88d22 100644 --- a/control/pure_pursuit/README.md +++ b/control/pure_pursuit/README.md @@ -6,14 +6,14 @@ The Pure Pursuit Controller module calculates the steering angle for tracking a Set the following from the [controller_node](../trajectory_follower_node/README.md) -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current ego pose and velocity information ## Outputs Return LateralOutput which contains the following to the controller node -- `autoware_auto_control_msgs/AckermannLateralCommand`: target steering angle +- `autoware_control_msgs/Lateral`: target steering angle - LateralSyncData - steer angle convergence -- `autoware_auto_planning_msgs/Trajectory`: predicted path for ego vehicle +- `autoware_planning_msgs/Trajectory`: predicted path for ego vehicle diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index ca0d77140b195..5b7b466dcb4dd 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -41,8 +41,8 @@ #include #include -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -56,9 +56,9 @@ using autoware::motion::control::trajectory_follower::InputData; using autoware::motion::control::trajectory_follower::LateralControllerBase; using autoware::motion::control::trajectory_follower::LateralOutput; -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; namespace pure_pursuit { @@ -107,20 +107,19 @@ class PurePursuitLateralController : public LateralControllerBase rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_; std::vector output_tp_array_; - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; - autoware_auto_planning_msgs::msg::Trajectory trajectory_; + autoware_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; + autoware_planning_msgs::msg::Trajectory trajectory_; nav_msgs::msg::Odometry current_odometry_; - autoware_auto_vehicle_msgs::msg::SteeringReport current_steering_; - boost::optional prev_cmd_; + autoware_vehicle_msgs::msg::SteeringReport current_steering_; + boost::optional prev_cmd_; // Debug Publisher rclcpp::Publisher::SharedPtr pub_debug_marker_; rclcpp::Publisher::SharedPtr pub_debug_values_; // Predicted Trajectory publish - rclcpp::Publisher::SharedPtr - pub_predicted_trajectory_; + rclcpp::Publisher::SharedPtr pub_predicted_trajectory_; - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); @@ -139,7 +138,7 @@ class PurePursuitLateralController : public LateralControllerBase bool isReady([[maybe_unused]] const InputData & input_data) override; LateralOutput run(const InputData & input_data) override; - AckermannLateralCommand generateCtrlCmdMsg(const double target_curvature); + Lateral generateCtrlCmdMsg(const double target_curvature); // Parameter Param param_{}; @@ -155,14 +154,13 @@ class PurePursuitLateralController : public LateralControllerBase * of vehicle. */ - TrajectoryPoint calcNextPose( - const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const; + TrajectoryPoint calcNextPose(const double ds, TrajectoryPoint & point, Lateral cmd) const; boost::optional generatePredictedTrajectory(); - AckermannLateralCommand generateOutputControlCmd(); + Lateral generateOutputControlCmd(); - bool calcIsSteerConverged(const AckermannLateralCommand & cmd); + bool calcIsSteerConverged(const Lateral & cmd); double calcLookaheadDistance( const double lateral_error, const double curvature, const double velocity, const double min_ld, @@ -170,7 +168,7 @@ class PurePursuitLateralController : public LateralControllerBase double calcCurvature(const size_t closest_idx); - void averageFilterTrajectory(autoware_auto_planning_msgs::msg::Trajectory & u); + void averageFilterTrajectory(autoware_planning_msgs::msg::Trajectory & u); // Debug mutable DebugData debug_data_; diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp index 6d9fca4b852cf..a5ae31133be8e 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp @@ -36,8 +36,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -79,15 +79,15 @@ class PurePursuitNode : public rclcpp::Node private: // Subscriber tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Subscription::SharedPtr sub_current_odometry_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; bool isDataReady(); - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); // TF @@ -96,8 +96,7 @@ class PurePursuitNode : public rclcpp::Node geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; // Publisher - rclcpp::Publisher::SharedPtr - pub_ctrl_cmd_; + rclcpp::Publisher::SharedPtr pub_ctrl_cmd_; void publishCommand(const double target_curvature); @@ -117,7 +116,7 @@ class PurePursuitNode : public rclcpp::Node std::unique_ptr pure_pursuit_; boost::optional calcTargetCurvature(); - boost::optional calcTargetPoint() const; + boost::optional calcTargetPoint() const; // Debug mutable DebugData debug_data_; diff --git a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp index a5b1e17ed983f..b2d6e13c585f4 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp +++ b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -51,7 +51,7 @@ namespace planning_utils { constexpr double ERROR = 1e-6; double calcArcLengthFromWayPoint( - const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, const size_t dst_idx); double calcCurvature( const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & curr_pose); @@ -66,7 +66,7 @@ double calcRadius( double convertCurvatureToSteeringAngle(double wheel_base, double kappa); std::vector extractPoses( - const autoware_auto_planning_msgs::msg::Trajectory & motions); + const autoware_planning_msgs::msg::Trajectory & motions); std::pair findClosestIdxWithDistAngThr( const std::vector & poses, diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index 30356b856fbac..b4820aee5cec4 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -13,8 +13,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_planning_msgs + autoware_control_msgs + autoware_planning_msgs boost geometry_msgs libboost-dev diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index f0c49b07e675a..a8232cce5d08d 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -95,7 +95,7 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) "~/debug/ld_outputs", rclcpp::QoS{1}); // Publish predicted trajectory - pub_predicted_trajectory_ = node.create_publisher( + pub_predicted_trajectory_ = node.create_publisher( "~/output/predicted_trajectory", 1); } @@ -138,7 +138,7 @@ double PurePursuitLateralController::calcLookaheadDistance( } TrajectoryPoint PurePursuitLateralController::calcNextPose( - const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const + const double ds, TrajectoryPoint & point, Lateral cmd) const { geometry_msgs::msg::Transform transform; transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); @@ -164,7 +164,7 @@ void PurePursuitLateralController::setResampledTrajectory() out_arclength.push_back(s); } trajectory_resampled_ = - std::make_shared(motion_utils::resampleTrajectory( + std::make_shared(motion_utils::resampleTrajectory( motion_utils::convertToTrajectory(input_tp_array), out_arclength)); trajectory_resampled_->points.back() = trajectory_.points.back(); trajectory_resampled_->header = trajectory_.header; @@ -215,14 +215,14 @@ double PurePursuitLateralController::calcCurvature(const size_t closest_idx) } void PurePursuitLateralController::averageFilterTrajectory( - autoware_auto_planning_msgs::msg::Trajectory & u) + autoware_planning_msgs::msg::Trajectory & u) { if (static_cast(u.points.size()) <= 2 * param_.path_filter_moving_ave_num) { RCLCPP_ERROR(logger_, "Cannot smooth path! Trajectory size is too low!"); return; } - autoware_auto_planning_msgs::msg::Trajectory filtered_trajectory(u); + autoware_planning_msgs::msg::Trajectory filtered_trajectory(u); for (int64_t i = 0; i < static_cast(u.points.size()); ++i) { TrajectoryPoint tmp{}; @@ -295,7 +295,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje predicted_trajectory.points.push_back(p); const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose); - AckermannLateralCommand tmp_msg; + Lateral tmp_msg; if (pp_output) { tmp_msg = generateCtrlCmdMsg(pp_output->curvature); @@ -310,7 +310,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje } else { const auto pp_output = calcTargetCurvature(false, predicted_trajectory.points.at(i).pose); - AckermannLateralCommand tmp_msg; + Lateral tmp_msg; if (pp_output) { tmp_msg = generateCtrlCmdMsg(pp_output->curvature); @@ -365,21 +365,21 @@ LateralOutput PurePursuitLateralController::run(const InputData & input_data) return output; } -bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCommand & cmd) +bool PurePursuitLateralController::calcIsSteerConverged(const Lateral & cmd) { return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) < static_cast(param_.converged_steer_rad_); } -AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() +Lateral PurePursuitLateralController::generateOutputControlCmd() { // Generate the control command const auto pp_output = calcTargetCurvature(true, current_odometry_.pose.pose); - AckermannLateralCommand output_cmd; + Lateral output_cmd; if (pp_output) { output_cmd = generateCtrlCmdMsg(pp_output->curvature); - prev_cmd_ = boost::optional(output_cmd); + prev_cmd_ = boost::optional(output_cmd); publishDebugMarker(); } else { RCLCPP_WARN_THROTTLE( @@ -393,12 +393,11 @@ AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() return output_cmd; } -AckermannLateralCommand PurePursuitLateralController::generateCtrlCmdMsg( - const double target_curvature) +Lateral PurePursuitLateralController::generateCtrlCmdMsg(const double target_curvature) { const double tmp_steering = planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); - AckermannLateralCommand cmd; + Lateral cmd; cmd.stamp = clock_->now(); cmd.steering_tire_angle = static_cast( std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle)); diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp index 254b83bccbc34..a4b491930df1e 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp @@ -73,14 +73,14 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) // Subscribers using std::placeholders::_1; - sub_trajectory_ = this->create_subscription( + sub_trajectory_ = this->create_subscription( "input/reference_trajectory", 1, std::bind(&PurePursuitNode::onTrajectory, this, _1)); sub_current_odometry_ = this->create_subscription( "input/current_odometry", 1, std::bind(&PurePursuitNode::onCurrentOdometry, this, _1)); // Publishers - pub_ctrl_cmd_ = this->create_publisher( - "output/control_raw", 1); + pub_ctrl_cmd_ = + this->create_publisher("output/control_raw", 1); // Debug Publishers pub_debug_marker_ = @@ -124,7 +124,7 @@ void PurePursuitNode::onCurrentOdometry(const nav_msgs::msg::Odometry::ConstShar } void PurePursuitNode::onTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { trajectory_ = msg; } @@ -150,7 +150,7 @@ void PurePursuitNode::onTimer() void PurePursuitNode::publishCommand(const double target_curvature) { - autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + autoware_control_msgs::msg::Lateral cmd; cmd.stamp = get_clock()->now(); cmd.steering_tire_angle = planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); @@ -211,8 +211,8 @@ boost::optional PurePursuitNode::calcTargetCurvature() return kappa; } -boost::optional -PurePursuitNode::calcTargetPoint() const +boost::optional PurePursuitNode::calcTargetPoint() + const { const auto closest_idx_result = planning_utils::findClosestIdxWithDistAngThr( planning_utils::extractPoses(*trajectory_), current_pose_->pose, 3.0, M_PI_4); diff --git a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp b/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp index 2597d7087ad61..b3a846157638a 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp +++ b/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp @@ -23,7 +23,7 @@ namespace pure_pursuit namespace planning_utils { double calcArcLengthFromWayPoint( - const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, const size_t dst_idx) { double length = 0; @@ -101,7 +101,7 @@ double convertCurvatureToSteeringAngle(double wheel_base, double kappa) } std::vector extractPoses( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) + const autoware_planning_msgs::msg::Trajectory & trajectory) { std::vector poses; diff --git a/control/shift_decider/README.md b/control/shift_decider/README.md index 820432e4c9e1a..c9fdc20696f0e 100644 --- a/control/shift_decider/README.md +++ b/control/shift_decider/README.md @@ -37,15 +37,15 @@ stop ### Input -| Name | Type | Description | -| --------------------- | ---------------------------------------------------------- | ---------------------------- | -| `~/input/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Control command for vehicle. | +| Name | Type | Description | +| --------------------- | ------------------------------------- | ---------------------------- | +| `~/input/control_cmd` | `autoware_control_msgs::msg::Control` | Control command for vehicle. | ### Output -| Name | Type | Description | -| ------------------ | ---------------------------------------------- | ---------------------------------- | -| `~output/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | +| Name | Type | Description | +| ------------------ | ----------------------------------------- | ---------------------------------- | +| `~output/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | ## Parameters diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp index a6b9938e404f6..b11a0f40625af 100644 --- a/control/shift_decider/include/shift_decider/shift_decider.hpp +++ b/control/shift_decider/include/shift_decider/shift_decider.hpp @@ -17,10 +17,10 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -31,26 +31,24 @@ class ShiftDecider : public rclcpp::Node private: void onTimer(); - void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg); - void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg); - void onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg); + void onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg); + void onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg); + void onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg); void updateCurrentShiftCmd(); void initTimer(double period_s); - rclcpp::Publisher::SharedPtr pub_shift_cmd_; - rclcpp::Subscription::SharedPtr - sub_control_cmd_; - rclcpp::Subscription::SharedPtr - sub_autoware_state_; - rclcpp::Subscription::SharedPtr sub_current_gear_; + rclcpp::Publisher::SharedPtr pub_shift_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_autoware_state_; + rclcpp::Subscription::SharedPtr sub_current_gear_; rclcpp::TimerBase::SharedPtr timer_; - autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr control_cmd_; - autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_; - autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_; - autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; - uint8_t prev_shift_command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK; + autoware_control_msgs::msg::Control::SharedPtr control_cmd_; + autoware_system_msgs::msg::AutowareState::SharedPtr autoware_state_; + autoware_vehicle_msgs::msg::GearCommand shift_cmd_; + autoware_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; + uint8_t prev_shift_command = autoware_vehicle_msgs::msg::GearCommand::PARK; bool park_on_goal_; }; diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml index b24dbab1786e1..885e780c90bcc 100644 --- a/control/shift_decider/package.xml +++ b/control/shift_decider/package.xml @@ -12,9 +12,9 @@ ament_cmake autoware_cmake - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_system_msgs + autoware_vehicle_msgs rclcpp rclcpp_components diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp index 0e47cc7378f27..f003513060a34 100644 --- a/control/shift_decider/src/shift_decider.cpp +++ b/control/shift_decider/src/shift_decider.cpp @@ -33,29 +33,28 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) park_on_goal_ = declare_parameter("park_on_goal"); pub_shift_cmd_ = - create_publisher("output/gear_cmd", durable_qos); - sub_control_cmd_ = create_subscription( + create_publisher("output/gear_cmd", durable_qos); + sub_control_cmd_ = create_subscription( "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); - sub_autoware_state_ = create_subscription( + sub_autoware_state_ = create_subscription( "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1)); - sub_current_gear_ = create_subscription( + sub_current_gear_ = create_subscription( "input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1)); initTimer(0.1); } -void ShiftDecider::onControlCmd( - autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg) +void ShiftDecider::onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg) { control_cmd_ = msg; } -void ShiftDecider::onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg) +void ShiftDecider::onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg) { autoware_state_ = msg; } -void ShiftDecider::onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) +void ShiftDecider::onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg) { current_gear_ptr_ = msg; } @@ -72,15 +71,15 @@ void ShiftDecider::onTimer() void ShiftDecider::updateCurrentShiftCmd() { - using autoware_auto_system_msgs::msg::AutowareState; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_system_msgs::msg::AutowareState; + using autoware_vehicle_msgs::msg::GearCommand; shift_cmd_.stamp = now(); static constexpr double vel_threshold = 0.01; // to prevent chattering if (autoware_state_->state == AutowareState::DRIVING) { - if (control_cmd_->longitudinal.speed > vel_threshold) { + if (control_cmd_->longitudinal.velocity > vel_threshold) { shift_cmd_.command = GearCommand::DRIVE; - } else if (control_cmd_->longitudinal.speed < -vel_threshold) { + } else if (control_cmd_->longitudinal.velocity < -vel_threshold) { shift_cmd_.command = GearCommand::REVERSE; } else { shift_cmd_.command = prev_shift_command; diff --git a/control/smart_mpc_trajectory_follower/package.xml b/control/smart_mpc_trajectory_follower/package.xml index 70bd15f97d609..6cdd689c8bd5a 100644 --- a/control/smart_mpc_trajectory_follower/package.xml +++ b/control/smart_mpc_trajectory_follower/package.xml @@ -17,10 +17,10 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs motion_utils pybind11_vendor python3-scipy diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index 409b6bf594c9e..992a5fd53a10c 100755 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -19,10 +19,10 @@ import time from autoware_adapi_v1_msgs.msg import OperationModeState -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_planning_msgs.msg import Trajectory -from autoware_auto_planning_msgs.msg import TrajectoryPoint -from autoware_auto_vehicle_msgs.msg import SteeringReport +from autoware_control_msgs.msg import Control +from autoware_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import TrajectoryPoint +from autoware_vehicle_msgs.msg import SteeringReport from builtin_interfaces.msg import Duration from geometry_msgs.msg import AccelWithCovarianceStamped from geometry_msgs.msg import PoseStamped @@ -128,7 +128,7 @@ def __init__(self): self.sub_reload_mpc_param_trigger_ self.sub_control_command_control_cmd_ = self.create_subscription( - AckermannControlCommand, + Control, "/control/command/control_cmd", self.onControlCommandControlCmd, 3, @@ -136,7 +136,7 @@ def __init__(self): self.sub_control_command_control_cmd_ self.control_cmd_pub_ = self.create_publisher( - AckermannControlCommand, + Control, "/control/trajectory_follower/control_cmd", 1, ) @@ -699,7 +699,7 @@ def control(self): else: steer_cmd = 0.0 - cmd_msg = AckermannControlCommand() + cmd_msg = Control() cmd_msg.stamp = cmd_msg.lateral.stamp = cmd_msg.longitudinal.stamp = ( self.get_clock().now().to_msg() ) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash index 524dfe5a169df..d2687b352a17d 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash +++ b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash @@ -2,16 +2,16 @@ gnome-terminal -- bash -c 'ros2 topic echo /localization/kinematic_state nav_msgs/msg/Odometry --csv --qos-history keep_all --qos-reliability reliable > kinematic_state.csv' gnome-terminal -- bash -c 'ros2 topic echo /localization/acceleration geometry_msgs/msg/AccelWithCovarianceStamped --csv --qos-history keep_all --qos-reliability reliable > acceleration.csv' -gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autoware_auto_vehicle_msgs/msg/SteeringReport --csv --qos-history keep_all --qos-reliability reliable > steering_status.csv' +gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autoware_vehicle_msgs/msg/SteeringReport --csv --qos-history keep_all --qos-reliability reliable > steering_status.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_auto_control_msgs/msg/AckermannControlCommand --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_auto_control_msgs/msg/AckermannControlCommand --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > lateral_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > yaw_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /system/operation_mode/state autoware_adapi_v1_msgs/msg/OperationModeState --csv --qos-history keep_all --qos-reliability reliable > system_operation_mode_state.csv' -gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_auto_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' +gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' gnome-terminal -- bash -c 'ros2 topic echo /sensing/imu/imu_data sensor_msgs/msg/Imu --csv --qos-history keep_all --qos-reliability reliable > imu.csv' gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_des.csv' diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp index 52a82526a9548..665a8604214dd 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp @@ -16,8 +16,8 @@ #define TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -25,9 +25,9 @@ namespace autoware::motion::control::trajectory_follower { struct InputData { - autoware_auto_planning_msgs::msg::Trajectory current_trajectory; + autoware_planning_msgs::msg::Trajectory current_trajectory; nav_msgs::msg::Odometry current_odometry; - autoware_auto_vehicle_msgs::msg::SteeringReport current_steering; + autoware_vehicle_msgs::msg::SteeringReport current_steering; geometry_msgs::msg::AccelWithCovarianceStamped current_accel; autoware_adapi_v1_msgs::msg::OperationModeState current_operation_mode; }; diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp index 657c981414c32..a70c4c18fedb3 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp @@ -19,7 +19,7 @@ #include "trajectory_follower_base/input_data.hpp" #include "trajectory_follower_base/sync_data.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include @@ -27,7 +27,7 @@ namespace autoware::motion::control::trajectory_follower { struct LateralOutput { - autoware_auto_control_msgs::msg::AckermannLateralCommand control_cmd; + autoware_control_msgs::msg::Lateral control_cmd; LateralSyncData sync_data; }; diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp index 0f9c0d57bb5cd..da5381091113f 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp @@ -19,7 +19,7 @@ #include "trajectory_follower_base/input_data.hpp" #include "trajectory_follower_base/sync_data.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" #include @@ -27,7 +27,7 @@ namespace autoware::motion::control::trajectory_follower { struct LongitudinalOutput { - autoware_auto_control_msgs::msg::LongitudinalCommand control_cmd; + autoware_control_msgs::msg::Longitudinal control_cmd; LongitudinalSyncData sync_data; }; class LongitudinalControllerBase diff --git a/control/trajectory_follower_base/package.xml b/control/trajectory_follower_base/package.xml index d896f874a3a20..6f2e9c3e8ebc2 100644 --- a/control/trajectory_follower_base/package.xml +++ b/control/trajectory_follower_base/package.xml @@ -20,9 +20,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md index c3ea3ddf6b7e8..3496284efd670 100644 --- a/control/trajectory_follower_node/README.md +++ b/control/trajectory_follower_node/README.md @@ -129,19 +129,19 @@ Giving the longitudinal controller information about steer convergence allows it #### Inputs -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport` current steering +- `autoware_vehicle_msgs/SteeringReport` current steering #### Outputs -- `autoware_auto_control_msgs/AckermannControlCommand`: message containing both lateral and longitudinal commands. +- `autoware_control_msgs/Control`: message containing both lateral and longitudinal commands. #### Parameter - `ctrl_period`: control commands publishing period - `timeout_thr_sec`: duration in second after which input messages are discarded. - - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `AckermannControlCommand` if the following two conditions are met. + - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `Control` if the following two conditions are met. 1. Both commands have been received. 2. The last received commands are not older than defined by `timeout_thr_sec`. - `lateral_controller_mode`: `mpc` or `pure_pursuit` diff --git a/control/trajectory_follower_node/design/simple_trajectory_follower-design.md b/control/trajectory_follower_node/design/simple_trajectory_follower-design.md index 24ffe3166bbe4..4c1ee1b422484 100644 --- a/control/trajectory_follower_node/design/simple_trajectory_follower-design.md +++ b/control/trajectory_follower_node/design/simple_trajectory_follower-design.md @@ -10,10 +10,10 @@ Provide a base trajectory follower code that is simple and flexible to use. This Inputs -- `input/reference_trajectory` [autoware_auto_planning_msgs::msg::Trajectory] : reference trajectory to follow. +- `input/reference_trajectory` [autoware_planning_msgs::msg::Trajectory] : reference trajectory to follow. - `input/current_kinematic_state` [nav_msgs::msg::Odometry] : current state of the vehicle (position, velocity, etc). - Output -- `output/control_cmd` [autoware_auto_control_msgs::msg::AckermannControlCommand] : generated control command. +- `output/control_cmd` [autoware_control_msgs::msg::Control] : generated control command. ### Parameters diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index be4fe885ae8c1..8e9752ba19f66 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -30,10 +30,9 @@ #include #include -#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -76,20 +75,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node std::shared_ptr longitudinal_controller_; std::shared_ptr lateral_controller_; - rclcpp::Subscription::SharedPtr sub_ref_path_; + rclcpp::Subscription::SharedPtr sub_ref_path_; rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr sub_steering_; + rclcpp::Subscription::SharedPtr sub_steering_; rclcpp::Subscription::SharedPtr sub_accel_; rclcpp::Subscription::SharedPtr sub_operation_mode_; - rclcpp::Publisher::SharedPtr - control_cmd_pub_; + rclcpp::Publisher::SharedPtr control_cmd_pub_; rclcpp::Publisher::SharedPtr pub_processing_time_lat_ms_; rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; rclcpp::Publisher::SharedPtr debug_marker_pub_; - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; + autoware_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_; - autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; + autoware_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr_; OperationModeState::SharedPtr current_operation_mode_ptr_; @@ -108,9 +106,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node */ boost::optional createInputData(rclcpp::Clock & clock) const; void callbackTimerControl(); - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr); void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); - void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); + void onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out); LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp index e748bdf25d419..e744243969cab 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp @@ -17,9 +17,9 @@ #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -28,9 +28,9 @@ namespace simple_trajectory_follower { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_control_msgs::msg::Control; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; @@ -44,7 +44,7 @@ class SimpleTrajectoryFollower : public rclcpp::Node private: rclcpp::Subscription::SharedPtr sub_kinematics_; rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::TimerBase::SharedPtr timer_; Trajectory::SharedPtr trajectory_; diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 36b4d0108de78..65446dfb3dd01 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -20,10 +20,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs motion_utils mpc_lateral_controller pid_longitudinal_controller diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 801f42ad9a470..6fe63ca07de6f 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -62,9 +62,9 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control throw std::domain_error("[LongitudinalController] invalid algorithm"); } - sub_ref_path_ = create_subscription( + sub_ref_path_ = create_subscription( "~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&Controller::onTrajectory, this, _1)); - sub_steering_ = create_subscription( + sub_steering_ = create_subscription( "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1)); sub_odometry_ = create_subscription( "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); @@ -73,7 +73,7 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control sub_operation_mode_ = create_subscription( "~/input/current_operation_mode", rclcpp::QoS{1}, [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; }); - control_cmd_pub_ = create_publisher( + control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); pub_processing_time_lat_ms_ = create_publisher("~/lateral/debug/processing_time_ms", 1); @@ -112,7 +112,7 @@ Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode return LongitudinalControllerMode::INVALID; } -void Controller::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +void Controller::onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { current_trajectory_ptr_ = msg; } @@ -122,7 +122,7 @@ void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) current_odometry_ptr_ = msg; } -void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) +void Controller::onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { current_steering_ptr_ = msg; } @@ -227,7 +227,7 @@ void Controller::callbackTimerControl() if (isTimeOut(lon_out, lat_out)) return; // 5. publish control command - autoware_auto_control_msgs::msg::AckermannControlCommand out; + autoware_control_msgs::msg::Control out; out.stamp = this->now(); out.lateral = lat_out.control_cmd; out.longitudinal = lon_out.control_cmd; diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp index 5e9488bf2ca93..7c27eaed41380 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -29,7 +29,7 @@ using tier4_autoware_utils::calcYawDeviation; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) { - pub_cmd_ = create_publisher("output/control_cmd", 1); + pub_cmd_ = create_publisher("output/control_cmd", 1); sub_kinematics_ = create_subscription( "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); @@ -54,11 +54,12 @@ void SimpleTrajectoryFollower::onTimer() updateClosest(); - AckermannControlCommand cmd; + Control cmd; cmd.stamp = cmd.lateral.stamp = cmd.longitudinal.stamp = get_clock()->now(); cmd.lateral.steering_tire_angle = static_cast(calcSteerCmd()); - cmd.longitudinal.speed = use_external_target_vel_ ? static_cast(external_target_vel_) - : closest_traj_point_.longitudinal_velocity_mps; + cmd.longitudinal.velocity = use_external_target_vel_ + ? static_cast(external_target_vel_) + : closest_traj_point_.longitudinal_velocity_mps; cmd.longitudinal.acceleration = static_cast(calcAccCmd()); pub_cmd_->publish(cmd); } diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp index 1aa4035e98d51..9bdf625226134 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -21,10 +21,9 @@ #include "trajectory_follower_test_utils.hpp" #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -35,11 +34,11 @@ #include using Controller = autoware::motion::control::trajectory_follower_node::Controller; -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; +using Control = autoware_control_msgs::msg::Control; +using Trajectory = autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; using VehicleOdometry = nav_msgs::msg::Odometry; -using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; +using SteeringReport = autoware_vehicle_msgs::msg::SteeringReport; using autoware_adapi_v1_msgs::msg::OperationModeState; using geometry_msgs::msg::AccelWithCovarianceStamped; @@ -96,7 +95,7 @@ class ControllerTester FakeNodeFixture * fnf; std::shared_ptr node; - AckermannControlCommand::SharedPtr cmd_msg; + Control::SharedPtr cmd_msg; bool received_control_command = false; void publish_default_odom() @@ -178,13 +177,11 @@ class ControllerTester rclcpp::Publisher::SharedPtr operation_mode_pub = fnf->create_publisher("controller/input/current_operation_mode"); - rclcpp::Subscription::SharedPtr cmd_sub = - fnf->create_subscription( - "controller/output/control_cmd", *fnf->get_fake_node(), - [this](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); + rclcpp::Subscription::SharedPtr cmd_sub = fnf->create_subscription( + "controller/output/control_cmd", *fnf->get_fake_node(), [this](const Control::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); std::shared_ptr br = std::make_shared(fnf->get_fake_node()); @@ -255,7 +252,7 @@ TEST_F(FakeNodeFixture, straight_trajectory) // following conditions will pass even if the MPC solution does not converge EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } @@ -369,14 +366,14 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0); EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); // Generate another control message tester.traj_pub->publish(traj_msg); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0); EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); } @@ -406,14 +403,14 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message tester.traj_pub->publish(traj); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -443,14 +440,14 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message tester.traj_pub->publish(traj); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -477,7 +474,7 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_LT( tester.cmd_msg->longitudinal.acceleration, 0.0f); // when stopped negative acceleration to brake @@ -507,7 +504,7 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -535,7 +532,7 @@ TEST_F(FakeNodeFixture, longitudinal_emergency) ASSERT_TRUE(tester.received_control_command); // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -566,7 +563,7 @@ TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged) ASSERT_TRUE(tester.received_control_command); // Not keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0f); } TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) @@ -597,5 +594,5 @@ TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) ASSERT_TRUE(tester.received_control_command); // Keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); } diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 6ec2da84a7b6c..2d6f5c5f949af 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -8,38 +8,38 @@ ### Input -| Name | Type | Description | -| ------------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------------------- | -| `~/input/steering` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | steering status | -| `~/input/auto/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from planning module | -| `~/input/auto/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from planning module | -| `~/input/auto/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from planning module | -| `~/input/auto/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from planning module | -| `~/input/external/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from external | -| `~/input/external/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from external | -| `~/input/external/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from external | -| `~/input/external/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from external | -| `~/input/external_emergency_stop_heartbeat` | `tier4_external_api_msgs::msg::Heartbeat` | heartbeat | -| `~/input/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | -| `~/input/emergency/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from emergency handler | -| `~/input/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | -| `~/input/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | -| `~/input/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | -| `~/input/operation_mode` | `autoware_adapi_v1_msgs::msg::OperationModeState` | operation mode of Autoware | +| Name | Type | Description | +| ------------------------------------------- | --------------------------------------------------- | -------------------------------------------------------------------- | +| `~/input/steering` | `autoware_vehicle_msgs::msg::SteeringReport` | steering status | +| `~/input/auto/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from planning module | +| `~/input/auto/turn_indicators_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from planning module | +| `~/input/auto/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from planning module | +| `~/input/auto/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command from planning module | +| `~/input/external/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from external | +| `~/input/external/turn_indicators_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from external | +| `~/input/external/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from external | +| `~/input/external/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command from external | +| `~/input/external_emergency_stop_heartbeat` | `tier4_external_api_msgs::msg::Heartbeat` | heartbeat | +| `~/input/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/input/emergency/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from emergency handler | +| `~/input/emergency/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | +| `~/input/emergency/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | +| `~/input/engage` | `autoware_vehicle_msgs::msg::Engage` | engage signal | +| `~/input/operation_mode` | `autoware_adapi_v1_msgs::msg::OperationModeState` | operation mode of Autoware | ### Output -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------- | -| `~/output/vehicle_cmd_emergency` | `autoware_auto_system_msgs::msg::EmergencyState` | emergency state which was originally in vehicle command | -| `~/output/command/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity to vehicle | -| `~/output/command/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command to vehicle | -| `~/output/command/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command to vehicle | -| `~/output/command/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command to vehicle | -| `~/output/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | -| `~/output/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | -| `~/output/external_emergency` | `tier4_external_api_msgs::msg::Emergency` | external emergency signal | -| `~/output/operation_mode` | `tier4_system_msgs::msg::OperationMode` | current operation mode of the vehicle_cmd_gate | +| Name | Type | Description | +| -------------------------------------- | --------------------------------------------------- | -------------------------------------------------------- | +| `~/output/vehicle_cmd_emergency` | `tier4_vehicle_msgs::msg::VehicleEmergencyStamped` | emergency state which was originally in vehicle command | +| `~/output/command/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity to vehicle | +| `~/output/command/turn_indicators_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command to vehicle | +| `~/output/command/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command to vehicle | +| `~/output/command/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command to vehicle | +| `~/output/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/output/engage` | `autoware_vehicle_msgs::msg::Engage` | engage signal | +| `~/output/external_emergency` | `tier4_external_api_msgs::msg::Emergency` | external emergency signal | +| `~/output/operation_mode` | `tier4_system_msgs::msg::OperationMode` | current operation mode of the vehicle_cmd_gate | ## Parameters diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index 22ae9da6d222e..e01d566c37df9 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -16,9 +16,8 @@ rosidl_default_generators autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils diagnostic_updater diff --git a/control/vehicle_cmd_gate/script/delays_checker.py b/control/vehicle_cmd_gate/script/delays_checker.py index 564b7c7ac94e5..a67c794877a92 100644 --- a/control/vehicle_cmd_gate/script/delays_checker.py +++ b/control/vehicle_cmd_gate/script/delays_checker.py @@ -14,8 +14,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_vehicle_msgs.msg import Engage +from autoware_control_msgs.msg import Control +from autoware_vehicle_msgs.msg import Engage from geometry_msgs.msg import AccelWithCovarianceStamped from nav_msgs.msg import Odometry import rclpy @@ -68,13 +68,13 @@ def __init__(self): ) self.sub_engage = self.create_subscription(Engage, engage_topic, self.CallBackEngage, 1) self.sub_in_gate_cmd = self.create_subscription( - AckermannControlCommand, + Control, in_gate_cmd_topic, self.CallBackInCmd, 1, ) self.sub_out_gate_cmd = self.create_subscription( - AckermannControlCommand, + Control, out_gate_cmd_topic, self.CallBackOutCmd, 1, diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp b/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp index afc86f0b4fb20..f4cf28d337a09 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp +++ b/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp @@ -53,9 +53,9 @@ void AdapiPauseInterface::publish() } } -void AdapiPauseInterface::update(const AckermannControlCommand & control) +void AdapiPauseInterface::update(const Control & control) { - is_start_requested_ = eps < std::abs(control.longitudinal.speed); + is_start_requested_ = eps < std::abs(control.longitudinal.velocity); } void AdapiPauseInterface::on_pause( diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp b/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp index c4294ee5f643d..1d5f05e98975e 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp +++ b/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include namespace vehicle_cmd_gate { @@ -28,7 +28,7 @@ class AdapiPauseInterface { private: static constexpr double eps = 1e-3; - using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + using Control = autoware_control_msgs::msg::Control; using SetPause = control_interface::SetPause; using IsPaused = control_interface::IsPaused; using IsStartRequested = control_interface::IsStartRequested; @@ -37,7 +37,7 @@ class AdapiPauseInterface explicit AdapiPauseInterface(rclcpp::Node * node); bool is_paused(); void publish(); - void update(const AckermannControlCommand & control); + void update(const Control & control); private: bool is_paused_; diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp b/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp index 7ef30b4926b7e..b643afc212f61 100644 --- a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp +++ b/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp @@ -19,8 +19,6 @@ #include #include -#include - #include #include diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index b289365b46b3b..bd9955e773020 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -95,24 +95,23 @@ VehicleCmdFilterParam VehicleCmdFilter::getParam() const return param_; } -void VehicleCmdFilter::limitLongitudinalWithVel(AckermannControlCommand & input) const +void VehicleCmdFilter::limitLongitudinalWithVel(Control & input) const { - input.longitudinal.speed = std::max( - std::min(static_cast(input.longitudinal.speed), param_.vel_lim), -param_.vel_lim); + input.longitudinal.velocity = std::max( + std::min(static_cast(input.longitudinal.velocity), param_.vel_lim), -param_.vel_lim); } -void VehicleCmdFilter::limitLongitudinalWithAcc( - const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLongitudinalWithAcc(const double dt, Control & input) const { const auto lon_acc_lim = getLonAccLim(); input.longitudinal.acceleration = std::max( std::min(static_cast(input.longitudinal.acceleration), lon_acc_lim), -lon_acc_lim); - input.longitudinal.speed = - limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, lon_acc_lim * dt); + input.longitudinal.velocity = + limitDiff(input.longitudinal.velocity, prev_cmd_.longitudinal.velocity, lon_acc_lim * dt); } void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( - const double dt, AckermannControlCommand & input) const + const double dt, Control & input) const { const auto lon_jerk_lim = getLonJerkLim(); input.longitudinal.acceleration = limitDiff( @@ -124,7 +123,7 @@ void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( // Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the // filtered steering angle oscillates if the input velocity oscillates. void VehicleCmdFilter::limitLateralWithLatAcc( - [[maybe_unused]] const double dt, AckermannControlCommand & input) const + [[maybe_unused]] const double dt, Control & input) const { const auto lat_acc_lim = getLatAccLim(); @@ -138,8 +137,7 @@ void VehicleCmdFilter::limitLateralWithLatAcc( // Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the // filtered steering angle oscillates if the input velocity oscillates. -void VehicleCmdFilter::limitLateralWithLatJerk( - const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralWithLatJerk(const double dt, Control & input) const { double curr_latacc = calcLatAcc(input, current_speed_); double prev_latacc = calcLatAcc(prev_cmd_, current_speed_); @@ -156,8 +154,7 @@ void VehicleCmdFilter::limitLateralWithLatJerk( } } -void VehicleCmdFilter::limitActualSteerDiff( - const double current_steer_angle, AckermannControlCommand & input) const +void VehicleCmdFilter::limitActualSteerDiff(const double current_steer_angle, Control & input) const { const auto actual_steer_diff_lim = getSteerDiffLim(); @@ -166,7 +163,7 @@ void VehicleCmdFilter::limitActualSteerDiff( input.lateral.steering_tire_angle = current_steer_angle + ds; } -void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralSteer(Control & input) const { const float steer_limit = getSteerLim(); @@ -185,7 +182,7 @@ void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const } } -void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralSteerRate(const double dt, Control & input) const { const float steer_rate_limit = getSteerRateLim(); @@ -201,7 +198,7 @@ void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCo } void VehicleCmdFilter::filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & cmd, + const double dt, const double current_steer_angle, Control & cmd, IsFilterActivated & is_activated) const { const auto cmd_orig = cmd; @@ -219,14 +216,14 @@ void VehicleCmdFilter::filterAll( } IsFilterActivated VehicleCmdFilter::checkIsActivated( - const AckermannControlCommand & c1, const AckermannControlCommand & c2, const double tol) + const Control & c1, const Control & c2, const double tol) { IsFilterActivated msg; msg.is_activated_on_steering = std::abs(c1.lateral.steering_tire_angle - c2.lateral.steering_tire_angle) > tol; msg.is_activated_on_steering_rate = std::abs(c1.lateral.steering_tire_rotation_rate - c2.lateral.steering_tire_rotation_rate) > tol; - msg.is_activated_on_speed = std::abs(c1.longitudinal.speed - c2.longitudinal.speed) > tol; + msg.is_activated_on_speed = std::abs(c1.longitudinal.velocity - c2.longitudinal.velocity) > tol; msg.is_activated_on_acceleration = std::abs(c1.longitudinal.acceleration - c2.longitudinal.acceleration) > tol; msg.is_activated_on_jerk = std::abs(c1.longitudinal.jerk - c2.longitudinal.jerk) > tol; @@ -244,13 +241,13 @@ double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc return std::atan(latacc * param_.wheel_base / v_sq); } -double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd) const +double VehicleCmdFilter::calcLatAcc(const Control & cmd) const { - double v = cmd.longitudinal.speed; + double v = cmd.longitudinal.velocity; return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } -double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd, const double v) const +double VehicleCmdFilter::calcLatAcc(const Control & cmd, const double v) const { return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index e71fb405beda1..d9b8383a1de51 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -18,13 +18,13 @@ #include #include -#include +#include #include namespace vehicle_cmd_gate { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using vehicle_cmd_gate::msg::IsFilterActivated; using LimitArray = std::vector; @@ -59,35 +59,33 @@ class VehicleCmdFilter void setCurrentSpeed(double v) { current_speed_ = v; } void setParam(const VehicleCmdFilterParam & p); VehicleCmdFilterParam getParam() const; - void setPrevCmd(const AckermannControlCommand & v) { prev_cmd_ = v; } + void setPrevCmd(const Control & v) { prev_cmd_ = v; } - void limitLongitudinalWithVel(AckermannControlCommand & input) const; - void limitLongitudinalWithAcc(const double dt, AckermannControlCommand & input) const; - void limitLongitudinalWithJerk(const double dt, AckermannControlCommand & input) const; - void limitLateralWithLatAcc(const double dt, AckermannControlCommand & input) const; - void limitLateralWithLatJerk(const double dt, AckermannControlCommand & input) const; - void limitActualSteerDiff( - const double current_steer_angle, AckermannControlCommand & input) const; - void limitLateralSteer(AckermannControlCommand & input) const; - void limitLateralSteerRate(const double dt, AckermannControlCommand & input) const; + void limitLongitudinalWithVel(Control & input) const; + void limitLongitudinalWithAcc(const double dt, Control & input) const; + void limitLongitudinalWithJerk(const double dt, Control & input) const; + void limitLateralWithLatAcc(const double dt, Control & input) const; + void limitLateralWithLatJerk(const double dt, Control & input) const; + void limitActualSteerDiff(const double current_steer_angle, Control & input) const; + void limitLateralSteer(Control & input) const; + void limitLateralSteerRate(const double dt, Control & input) const; void filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & input, + const double dt, const double current_steer_angle, Control & input, IsFilterActivated & is_activated) const; static IsFilterActivated checkIsActivated( - const AckermannControlCommand & c1, const AckermannControlCommand & c2, - const double tol = 1.0e-3); + const Control & c1, const Control & c2, const double tol = 1.0e-3); - AckermannControlCommand getPrevCmd() { return prev_cmd_; } + Control getPrevCmd() { return prev_cmd_; } private: VehicleCmdFilterParam param_; - AckermannControlCommand prev_cmd_; + Control prev_cmd_; double current_speed_ = 0.0; bool setParameterWithValidation(const VehicleCmdFilterParam & p); - double calcLatAcc(const AckermannControlCommand & cmd) const; - double calcLatAcc(const AckermannControlCommand & cmd, const double v) const; + double calcLatAcc(const Control & cmd) const; + double calcLatAcc(const Control & cmd, const double v) const; double calcSteerFromLatacc(const double v, const double latacc) const; double limitDiff(const double curr, const double prev, const double diff_lim) const; diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index ffd452db0337c..cc9d7813083f4 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -62,7 +62,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) // Publisher vehicle_cmd_emergency_pub_ = create_publisher("output/vehicle_cmd_emergency", durable_qos); - control_cmd_pub_ = create_publisher("output/control_cmd", durable_qos); + control_cmd_pub_ = create_publisher("output/control_cmd", durable_qos); gear_cmd_pub_ = create_publisher("output/gear_cmd", durable_qos); turn_indicator_cmd_pub_ = create_publisher("output/turn_indicators_cmd", durable_qos); @@ -108,7 +108,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); // Subscriber for auto - auto_control_cmd_sub_ = create_subscription( + auto_control_cmd_sub_ = create_subscription( "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1)); auto_turn_indicator_cmd_sub_ = create_subscription( @@ -124,7 +124,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) [this](GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; }); // Subscriber for external - remote_control_cmd_sub_ = create_subscription( + remote_control_cmd_sub_ = create_subscription( "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1)); remote_turn_indicator_cmd_sub_ = create_subscription( @@ -140,7 +140,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) [this](GearCommand::ConstSharedPtr msg) { remote_commands_.gear = *msg; }); // Subscriber for emergency - emergency_control_cmd_sub_ = create_subscription( + emergency_control_cmd_sub_ = create_subscription( "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1)); emergency_hazard_light_cmd_sub_ = create_subscription( @@ -354,7 +354,7 @@ bool VehicleCmdGate::isDataReady() } // for auto -void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onAutoCtrlCmd(Control::ConstSharedPtr msg) { auto_commands_.control = *msg; @@ -364,7 +364,7 @@ void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) } // for remote -void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onRemoteCtrlCmd(Control::ConstSharedPtr msg) { remote_commands_.control = *msg; @@ -374,7 +374,7 @@ void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg } // for emergency -void VehicleCmdGate::onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg) { emergency_commands_.control = *msg; @@ -489,7 +489,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) } if (moderate_stop_interface_->is_stop_requested()) { // if stop requested, stop the vehicle - filtered_commands.control.longitudinal.speed = 0.0; + filtered_commands.control.longitudinal.velocity = 0.0; filtered_commands.control.longitudinal.acceleration = moderate_stop_service_acceleration_; } @@ -543,7 +543,7 @@ void VehicleCmdGate::publishEmergencyStopControlCommands() const auto stamp = this->now(); // ControlCommand - AckermannControlCommand control_cmd; + Control control_cmd; control_cmd.stamp = stamp; control_cmd = createEmergencyStopControlCmd(); @@ -600,9 +600,9 @@ void VehicleCmdGate::publishStatus() moderate_stop_interface_->publish(); } -AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannControlCommand & in) +Control VehicleCmdGate::filterControlCommand(const Control & in) { - AckermannControlCommand out = in; + Control out = in; const double dt = getDt(); const auto mode = current_operation_mode_; const auto current_status_cmd = getActualStatusAsCommand(); @@ -629,10 +629,10 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont prev_cmd.longitudinal.acceleration = std::max(prev_cmd.longitudinal.acceleration, current_status_cmd.longitudinal.acceleration); // consider reverse driving - prev_cmd.longitudinal.speed = - std::fabs(prev_cmd.longitudinal.speed) > std::fabs(current_status_cmd.longitudinal.speed) - ? prev_cmd.longitudinal.speed - : current_status_cmd.longitudinal.speed; + prev_cmd.longitudinal.velocity = std::fabs(prev_cmd.longitudinal.velocity) > + std::fabs(current_status_cmd.longitudinal.velocity) + ? prev_cmd.longitudinal.velocity + : current_status_cmd.longitudinal.velocity; filter_.setPrevCmd(prev_cmd); } filter_.filterAll(dt, current_steer_, out, is_filter_activated); @@ -663,42 +663,42 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont return out; } -AckermannControlCommand VehicleCmdGate::createStopControlCmd() const +Control VehicleCmdGate::createStopControlCmd() const { - AckermannControlCommand cmd; + Control cmd; const auto t = this->now(); cmd.stamp = t; cmd.lateral.stamp = t; cmd.longitudinal.stamp = t; cmd.lateral.steering_tire_angle = current_steer_; cmd.lateral.steering_tire_rotation_rate = 0.0; - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = stop_hold_acceleration_; return cmd; } -LongitudinalCommand VehicleCmdGate::createLongitudinalStopControlCmd() const +Longitudinal VehicleCmdGate::createLongitudinalStopControlCmd() const { - LongitudinalCommand cmd; + Longitudinal cmd; const auto t = this->now(); cmd.stamp = t; - cmd.speed = 0.0; + cmd.velocity = 0.0; cmd.acceleration = stop_hold_acceleration_; return cmd; } -AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const +Control VehicleCmdGate::createEmergencyStopControlCmd() const { - AckermannControlCommand cmd; + Control cmd; const auto t = this->now(); cmd.stamp = t; cmd.lateral.stamp = t; cmd.longitudinal.stamp = t; cmd.lateral.steering_tire_angle = prev_control_cmd_.lateral.steering_tire_angle; cmd.lateral.steering_tire_rotation_rate = prev_control_cmd_.lateral.steering_tire_rotation_rate; - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = emergency_acceleration_; return cmd; @@ -757,13 +757,13 @@ double VehicleCmdGate::getDt() return dt; } -AckermannControlCommand VehicleCmdGate::getActualStatusAsCommand() +Control VehicleCmdGate::getActualStatusAsCommand() { - AckermannControlCommand status; + Control status; status.stamp = status.lateral.stamp = status.longitudinal.stamp = this->now(); status.lateral.steering_tire_angle = current_steer_; status.lateral.steering_tire_rotation_rate = 0.0; - status.longitudinal.speed = current_kinematics_.twist.twist.linear.x; + status.longitudinal.velocity = current_kinematics_.twist.twist.linear.x; status.longitudinal.acceleration = current_acceleration_; return status; } diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 85bc183361b94..f45280d9d2e48 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -29,12 +29,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -56,12 +56,12 @@ namespace vehicle_cmd_gate using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_control_msgs::msg::LongitudinalCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_control_msgs::msg::Control; +using autoware_control_msgs::msg::Longitudinal; +using autoware_vehicle_msgs::msg::GearCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using std_srvs::srv::Trigger; using tier4_control_msgs::msg::GateMode; @@ -77,13 +77,13 @@ using visualization_msgs::msg::MarkerArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; -using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; +using EngageMsg = autoware_vehicle_msgs::msg::Engage; using EngageSrv = tier4_external_api_msgs::srv::Engage; using motion_utils::VehicleStopChecker; struct Commands { - AckermannControlCommand control; + Control control; TurnIndicatorsCommand turn_indicator; HazardLightsCommand hazard_light; GearCommand gear; @@ -101,7 +101,7 @@ class VehicleCmdGate : public rclcpp::Node private: // Publisher rclcpp::Publisher::SharedPtr vehicle_cmd_emergency_pub_; - rclcpp::Publisher::SharedPtr control_cmd_pub_; + rclcpp::Publisher::SharedPtr control_cmd_pub_; rclcpp::Publisher::SharedPtr gear_cmd_pub_; rclcpp::Publisher::SharedPtr turn_indicator_cmd_pub_; rclcpp::Publisher::SharedPtr hazard_light_cmd_pub_; @@ -153,26 +153,26 @@ class VehicleCmdGate : public rclcpp::Node // Subscriber for auto Commands auto_commands_; - rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; + rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; rclcpp::Subscription::SharedPtr auto_turn_indicator_cmd_sub_; rclcpp::Subscription::SharedPtr auto_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr auto_gear_cmd_sub_; - void onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); + void onAutoCtrlCmd(Control::ConstSharedPtr msg); // Subscription for external Commands remote_commands_; - rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; + rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; rclcpp::Subscription::SharedPtr remote_turn_indicator_cmd_sub_; rclcpp::Subscription::SharedPtr remote_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr remote_gear_cmd_sub_; - void onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); + void onRemoteCtrlCmd(Control::ConstSharedPtr msg); // Subscription for emergency Commands emergency_commands_; - rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; + rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; rclcpp::Subscription::SharedPtr emergency_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr emergency_gear_cmd_sub_; - void onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); + void onEmergencyCtrlCmd(Control::ConstSharedPtr msg); // Parameter bool use_emergency_handling_; @@ -224,17 +224,17 @@ class VehicleCmdGate : public rclcpp::Node void checkExternalEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat); // Algorithm - AckermannControlCommand prev_control_cmd_; - AckermannControlCommand createStopControlCmd() const; - LongitudinalCommand createLongitudinalStopControlCmd() const; - AckermannControlCommand createEmergencyStopControlCmd() const; + Control prev_control_cmd_; + Control createStopControlCmd() const; + Longitudinal createLongitudinalStopControlCmd() const; + Control createEmergencyStopControlCmd() const; std::shared_ptr prev_time_; double getDt(); - AckermannControlCommand getActualStatusAsCommand(); + Control getActualStatusAsCommand(); VehicleCmdFilter filter_; - AckermannControlCommand filterControlCommand(const AckermannControlCommand & msg); + Control filterControlCommand(const Control & msg); // filtering on transition OperationModeState current_operation_mode_; diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 949ca46d27dea..d51db90c8a260 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -58,26 +58,25 @@ using vehicle_cmd_gate::VehicleCmdGate; using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_control_msgs::msg::Control; +using autoware_vehicle_msgs::msg::GearCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using nav_msgs::msg::Odometry; using tier4_control_msgs::msg::GateMode; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; -using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; +using EngageMsg = autoware_vehicle_msgs::msg::Engage; class PubSubNode : public rclcpp::Node { public: PubSubNode() : Node{"test_vehicle_cmd_gate_filter_pubsub"} { - sub_cmd_ = create_subscription( - "output/control_cmd", rclcpp::QoS{1}, - [this](const AckermannControlCommand::ConstSharedPtr msg) { + sub_cmd_ = create_subscription( + "output/control_cmd", rclcpp::QoS{1}, [this](const Control::ConstSharedPtr msg) { cmd_history_.push_back(msg); cmd_received_times_.push_back(now()); // check the effectiveness of the filter for last x elements in the queue @@ -97,8 +96,7 @@ class PubSubNode : public rclcpp::Node pub_operation_mode_ = create_publisher("input/operation_mode", qos); pub_mrm_state_ = create_publisher("input/mrm_state", qos); - pub_auto_control_cmd_ = - create_publisher("input/auto/control_cmd", qos); + pub_auto_control_cmd_ = create_publisher("input/auto/control_cmd", qos); pub_auto_turn_indicator_cmd_ = create_publisher("input/auto/turn_indicators_cmd", qos); pub_auto_hazard_light_cmd_ = @@ -106,7 +104,7 @@ class PubSubNode : public rclcpp::Node pub_auto_gear_cmd_ = create_publisher("input/auto/gear_cmd", qos); } - rclcpp::Subscription::SharedPtr sub_cmd_; + rclcpp::Subscription::SharedPtr sub_cmd_; rclcpp::Publisher::SharedPtr pub_external_emergency_stop_heartbeat_; rclcpp::Publisher::SharedPtr pub_engage_; @@ -116,13 +114,13 @@ class PubSubNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_steer_; rclcpp::Publisher::SharedPtr pub_operation_mode_; rclcpp::Publisher::SharedPtr pub_mrm_state_; - rclcpp::Publisher::SharedPtr pub_auto_control_cmd_; + rclcpp::Publisher::SharedPtr pub_auto_control_cmd_; rclcpp::Publisher::SharedPtr pub_auto_turn_indicator_cmd_; rclcpp::Publisher::SharedPtr pub_auto_hazard_light_cmd_; rclcpp::Publisher::SharedPtr pub_auto_gear_cmd_; - std::vector cmd_history_; - std::vector raw_cmd_history_; + std::vector cmd_history_; + std::vector raw_cmd_history_; std::vector cmd_received_times_; // publish except for the control_cmd @@ -152,7 +150,7 @@ class PubSubNode : public rclcpp::Node msg.twist.twist.linear.x = 0.0; if (!cmd_history_.empty()) { // ego moves as commanded. msg.twist.twist.linear.x = - cmd_history_.back()->longitudinal.speed; // ego moves as commanded. + cmd_history_.back()->longitudinal.velocity; // ego moves as commanded. } pub_odom_->publish(msg); } @@ -209,11 +207,11 @@ class PubSubNode : public rclcpp::Node } } - void publishControlCommand(AckermannControlCommand msg) + void publishControlCommand(Control msg) { msg.stamp = now(); pub_auto_control_cmd_->publish(msg); - raw_cmd_history_.push_back(std::make_shared(msg)); + raw_cmd_history_.push_back(std::make_shared(msg)); } void checkFilter(size_t last_x) @@ -249,7 +247,7 @@ class PubSubNode : public rclcpp::Node // TODO(Horibe): prev_lat_acc should use the previous velocity, but use the current velocity // since the current filtering logic uses the current velocity. // when it's fixed, should be like this: - // prev_lat_acc = calculate_lateral_acceleration(cmd_start->longitudinal.speed, + // prev_lat_acc = calculate_lateral_acceleration(cmd_start->longitudinal.velocity, // cmd_start->lateral.steering_tire_angle, wheelbase); prev_tire_angle = cmd_start->lateral.steering_tire_angle; } @@ -261,7 +259,7 @@ class PubSubNode : public rclcpp::Node ASSERT_GT(dt, 0.0) << "Invalid dt. Time must be strictly positive."; - lon_vel = cmd->longitudinal.speed; + lon_vel = cmd->longitudinal.velocity; const auto lon_acc = cmd->longitudinal.acceleration; const auto lon_jerk = (lon_acc - prev_lon_acc) / dt; @@ -341,7 +339,7 @@ class ControlCmdGenerator // generate ControlCommand with sin wave format. // TODO(Horibe): implement steering_rate and jerk command. - AckermannControlCommand calcSinWaveCommand(bool reset_clock = false) + Control calcSinWaveCommand(bool reset_clock = false) { if (reset_clock) { start_time_ = Clock::now(); @@ -355,9 +353,9 @@ class ControlCmdGenerator return amp * std::sin(2.0 * M_PI * freq * dt_s + bias); }; - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = sinWave(p_.steering.max, p_.steering.freq, p_.steering.bias); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = sinWave(p_.velocity.max, p_.velocity.freq, p_.velocity.bias) + p_.velocity.max; cmd.longitudinal.acceleration = sinWave(p_.acceleration.max, p_.acceleration.freq, p_.acceleration.bias); diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index b8200490dd1d5..5fbab1c5cfb4f 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -24,7 +24,7 @@ #define ASSERT_LT_NEAR(x, y) ASSERT_LT(x, y + THRESHOLD) #define ASSERT_GT_NEAR(x, y) ASSERT_GT(x, y - THRESHOLD) -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using vehicle_cmd_gate::LimitArray; constexpr double NOMINAL_INTERVAL = 1.0; @@ -49,38 +49,37 @@ void setFilterParams( f.setParam(p); } -AckermannControlCommand genCmd(double s, double sr, double v, double a) +Control genCmd(double s, double sr, double v, double a) { - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = s; cmd.lateral.steering_tire_rotation_rate = sr; - cmd.longitudinal.speed = v; + cmd.longitudinal.velocity = v; cmd.longitudinal.acceleration = a; return cmd; } // calc from ego velocity -double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase, const double ego_v) +double calcLatAcc(const Control & cmd, const double wheelbase, const double ego_v) { return ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } // calc from command velocity -double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase) +double calcLatAcc(const Control & cmd, const double wheelbase) { - double v = cmd.longitudinal.speed; + double v = cmd.longitudinal.velocity; return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } // calc from command velocity double calcLatJerk( - const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, - const double wheelbase, const double dt) + const Control & cmd, const Control & prev_cmd, const double wheelbase, const double dt) { - const auto prev_v = prev_cmd.longitudinal.speed; + const auto prev_v = prev_cmd.longitudinal.velocity; const auto prev = prev_v * prev_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; - const auto curr_v = cmd.longitudinal.speed; + const auto curr_v = cmd.longitudinal.velocity; const auto curr = curr_v * curr_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; return (curr - prev) / dt; @@ -88,8 +87,8 @@ double calcLatJerk( // calc from ego velocity double calcLatJerk( - const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, - const double wheelbase, const double dt, const double ego_v) + const Control & cmd, const Control & prev_cmd, const double wheelbase, const double dt, + const double ego_v) { const auto prev = ego_v * ego_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; @@ -100,8 +99,8 @@ double calcLatJerk( void test_1d_limit( double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, - double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, - const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) + double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, const Control & prev_cmd, + const Control & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] @@ -119,11 +118,11 @@ void test_1d_limit( filter.limitLongitudinalWithVel(filtered_cmd); // check if the filtered value does not exceed the limit. - ASSERT_LT_NEAR(filtered_cmd.longitudinal.speed, V_LIM); + ASSERT_LT_NEAR(filtered_cmd.longitudinal.velocity, V_LIM); // check if the undesired filter is not applied. - if (std::abs(raw_cmd.longitudinal.speed) < V_LIM) { - ASSERT_NEAR(filtered_cmd.longitudinal.speed, raw_cmd.longitudinal.speed, THRESHOLD); + if (std::abs(raw_cmd.longitudinal.velocity) < V_LIM) { + ASSERT_NEAR(filtered_cmd.longitudinal.velocity, raw_cmd.longitudinal.velocity, THRESHOLD); } } @@ -145,14 +144,14 @@ void test_1d_limit( } // check if the filtered value does not exceed the limit. - const double v_max = prev_cmd.longitudinal.speed + A_LIM * DT; - const double v_min = prev_cmd.longitudinal.speed - A_LIM * DT; - ASSERT_LT_NEAR(filtered_cmd.longitudinal.speed, v_max); - ASSERT_GT_NEAR(filtered_cmd.longitudinal.speed, v_min); + const double v_max = prev_cmd.longitudinal.velocity + A_LIM * DT; + const double v_min = prev_cmd.longitudinal.velocity - A_LIM * DT; + ASSERT_LT_NEAR(filtered_cmd.longitudinal.velocity, v_max); + ASSERT_GT_NEAR(filtered_cmd.longitudinal.velocity, v_min); // check if the undesired filter is not applied. - if (v_min < raw_cmd.longitudinal.speed && raw_cmd.longitudinal.speed < v_max) { - ASSERT_NEAR(filtered_cmd.longitudinal.speed, raw_cmd.longitudinal.speed, THRESHOLD); + if (v_min < raw_cmd.longitudinal.velocity && raw_cmd.longitudinal.velocity < v_max) { + ASSERT_NEAR(filtered_cmd.longitudinal.velocity, raw_cmd.longitudinal.velocity, THRESHOLD); } } @@ -241,10 +240,10 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector steer_rate_lim_arr = {0.01, 1.0, 100.0}; const std::vector ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0}; - const std::vector prev_cmd_arr = { + const std::vector prev_cmd_arr = { genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)}; - const std::vector raw_cmd_arr = { + const std::vector raw_cmd_arr = { genCmd(1.0, 1.0, 1.0, 1.0), genCmd(10.0, -1.0, -1.0, -1.0)}; for (const auto & v : v_arr) { @@ -294,10 +293,10 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto DT = 0.033; const auto orig_cmd = []() { - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = 0.5; cmd.lateral.steering_tire_rotation_rate = 0.5; - cmd.longitudinal.speed = 30.0; + cmd.longitudinal.velocity = 30.0; cmd.longitudinal.acceleration = 10.0; cmd.longitudinal.jerk = 10.0; return cmd; @@ -353,7 +352,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // vel lim { set_speed_and_reset_prev(0.0); - EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.speed, 20.0, ep); + EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.velocity, 20.0, ep); } // steer angle lim @@ -389,7 +388,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto calcSteerRateFromAngle = [&](const auto & cmd) { return (cmd.steering_tire_angle - 0.0) / DT; }; - autoware_auto_control_msgs::msg::AckermannLateralCommand filtered; + autoware_control_msgs::msg::Lateral filtered; set_speed_and_reset_prev(0.0); filtered = _limitSteerRate(orig_cmd).lateral; @@ -521,7 +520,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; // p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; const auto _calcLatJerk = [&](const auto & cmd, const double ego_v) { - return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT, ego_v); + return calcLatJerk(cmd, Control{}, WHEELBASE, DT, ego_v); }; { // since the lateral acceleration and jerk is 0 when the velocity is 0, the target value is 0 diff --git a/evaluator/kinematic_evaluator/package.xml b/evaluator/kinematic_evaluator/package.xml index 7129221f6c27b..1b62b06e57c9f 100644 --- a/evaluator/kinematic_evaluator/package.xml +++ b/evaluator/kinematic_evaluator/package.xml @@ -17,8 +17,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_planning_msgs diagnostic_msgs eigen geometry_msgs diff --git a/evaluator/localization_evaluator/package.xml b/evaluator/localization_evaluator/package.xml index 075bc83a1f985..ac52f9fe94d5e 100644 --- a/evaluator/localization_evaluator/package.xml +++ b/evaluator/localization_evaluator/package.xml @@ -12,8 +12,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_planning_msgs diagnostic_msgs eigen geometry_msgs diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md index 7ee93d2ffddf2..7df375ac236d0 100644 --- a/evaluator/perception_online_evaluator/README.md +++ b/evaluator/perception_online_evaluator/README.md @@ -155,11 +155,11 @@ where: ## Inputs / Outputs -| Name | Type | Description | -| ----------------- | ------------------------------------------------------ | ------------------------------------------------- | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. | -| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. | -| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. | +| Name | Type | Description | +| ----------------- | ------------------------------------------------- | ------------------------------------------------- | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. | +| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. | +| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. | ## Parameters diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp index 5715f22669fdc..010f1497da3da 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp @@ -21,8 +21,8 @@ #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include #include @@ -37,8 +37,8 @@ namespace perception_diagnostics { namespace metrics { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; struct DetectionRange { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp index da7a23b6980b6..b480eaa3e67d6 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp @@ -17,7 +17,7 @@ #include "perception_online_evaluator/stat.hpp" -#include +#include #include #include @@ -26,7 +26,7 @@ namespace perception_diagnostics { namespace metrics { -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Pose; /** 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 96b1c01ee2d16..ec435b0a5e17f 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 @@ -25,8 +25,8 @@ #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "geometry_msgs/msg/pose.hpp" #include @@ -41,8 +41,8 @@ namespace perception_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using metrics::DetectionCounter; 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 e5c41ff28c4da..daaea56178873 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 @@ -22,7 +22,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "nav_msgs/msg/odometry.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -35,8 +35,8 @@ namespace perception_diagnostics { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp index 0831d580248d3..be335a2d78fa3 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -17,7 +17,7 @@ #include -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include #include @@ -30,7 +30,7 @@ namespace marker_utils { -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using std_msgs::msg::ColorRGBA; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp index ee348b108d139..7adab46f42d2f 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp @@ -17,9 +17,9 @@ #include "perception_online_evaluator/parameters.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -34,9 +34,9 @@ namespace perception_diagnostics { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using ClassObjectsMap = std::unordered_map; diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml index 807f247807d98..16d4b6fe458cd 100644 --- a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml +++ b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml @@ -13,7 +13,7 @@ prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] stopped_velocity_threshold: 1.0 - detection_radius_list: [50.0, 100.0, 150.0, 200.0] + detection_radius_list: [20.0, 40.0, 60.0, 80.0, 100.0, 120.0, 140.0, 160.0, 180.0, 200.0] detection_height_list: [10.0] detection_count_purge_seconds: 36000.0 objects_count_window_seconds: 1.0 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 95d6f07cca5d9..a408821466fb9 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 @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -35,11 +35,11 @@ #include using EvalNode = perception_diagnostics::PerceptionOnlineEvaluatorNode; -using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; -using PredictedObject = autoware_auto_perception_msgs::msg::PredictedObject; +using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; +using PredictedObject = autoware_perception_msgs::msg::PredictedObject; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; using MarkerArray = visualization_msgs::msg::MarkerArray; -using ObjectClassification = autoware_auto_perception_msgs::msg::ObjectClassification; +using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; using nav_msgs::msg::Odometry; using TFMessage = tf2_msgs::msg::TFMessage; @@ -172,7 +172,7 @@ class EvalTest : public ::testing::Test object.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; object.kinematics.initial_twist_with_covariance.twist.linear.z = 0.0; - autoware_auto_perception_msgs::msg::PredictedPath path; + autoware_perception_msgs::msg::PredictedPath path; for (size_t i = 0; i < predicted_path.size(); ++i) { geometry_msgs::msg::Pose pose; pose.position.x = predicted_path[i].first; diff --git a/evaluator/planning_evaluator/README.md b/evaluator/planning_evaluator/README.md index 23c8101429c35..eeeba99da9b8b 100644 --- a/evaluator/planning_evaluator/README.md +++ b/evaluator/planning_evaluator/README.md @@ -48,11 +48,11 @@ Adding a new metric `M` requires the following steps: ### Inputs -| Name | Type | Description | -| ------------------------------ | ------------------------------------------------------ | ------------------------------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Main trajectory to evaluate | -| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory to use for deviation metrics | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Obstacles | +| Name | Type | Description | +| ------------------------------ | ------------------------------------------------- | ------------------------------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Main trajectory to evaluate | +| `~/input/reference_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory to use for deviation metrics | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Obstacles | ### Outputs diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp index 04a5b758d62e1..02464207801cb 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp @@ -17,15 +17,15 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp index 4806446d4270f..e3280187ccef7 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp @@ -17,8 +17,8 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" namespace planning_diagnostics { @@ -26,7 +26,7 @@ namespace metrics { namespace utils { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; /** * @brief find the index in the trajectory at the given distance of the given index diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp index 848d92c91f13e..062ace8747253 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp @@ -17,15 +17,15 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; /** * @brief calculate the distance to the closest obstacle at each point of the trajectory diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp index c4bf1fe901604..e6ae9222d4bb9 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp @@ -17,13 +17,13 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; /** * @brief calculate the discrete Frechet distance between two trajectories diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp index b55ad245d8425..e88864016ca84 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp @@ -17,15 +17,15 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; /** * @brief calculate relative angle metric (angle between successive points) diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp index 851678e55d755..99ec5b787706b 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp @@ -18,20 +18,20 @@ #include "planning_evaluator/parameters.hpp" #include "planning_evaluator/stat.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include namespace planning_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp index 28a4b2cba8365..8e2561e3eaf0f 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp @@ -21,8 +21,8 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include @@ -33,8 +33,8 @@ namespace planning_diagnostics { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; /** * @brief Node for planning evaluation diff --git a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp index 888eea855295c..e02f833a67291 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp @@ -21,10 +21,10 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -36,14 +36,13 @@ namespace planning_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; - /** * @brief Node for planning evaluation */ diff --git a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml index 1b7510c2ced69..4f36a614b5d09 100644 --- a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/evaluator/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml index fb3270fb0d89b..cf91d7077d609 100644 --- a/evaluator/planning_evaluator/package.xml +++ b/evaluator/planning_evaluator/package.xml @@ -13,8 +13,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs autoware_planning_msgs diagnostic_msgs eigen diff --git a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp index e1ad9c1a2eeec..e403b8415938d 100644 --- a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp @@ -22,8 +22,8 @@ namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj) { diff --git a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp index 6134a100f31a4..974f30223bff8 100644 --- a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp @@ -20,8 +20,8 @@ namespace metrics { namespace utils { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::calcDistance2d; size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance) diff --git a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp index 82d6dcc51097e..af4508a370ab6 100644 --- a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -18,7 +18,7 @@ #include -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -27,7 +27,7 @@ namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::calcDistance2d; Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) diff --git a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp index 7a3418da881c6..c963c5e46aa43 100644 --- a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp @@ -19,7 +19,7 @@ #include -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include @@ -27,7 +27,7 @@ namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2) { diff --git a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp index 4f2ab014a79d6..e87b5c8ad35b7 100644 --- a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp @@ -20,8 +20,8 @@ #include -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -33,9 +33,9 @@ #include using EvalNode = planning_diagnostics::PlanningEvaluatorNode; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using Objects = autoware_auto_perception_msgs::msg::PredictedObjects; +using Trajectory = autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; +using Objects = autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using diagnostic_msgs::msg::DiagnosticArray; using nav_msgs::msg::Odometry; @@ -409,7 +409,7 @@ TEST_F(EvalTest, TestObstacleDistance) { setTargetMetric(planning_diagnostics::Metric::obstacle_distance); Objects objs; - autoware_auto_perception_msgs::msg::PredictedObject obj; + autoware_perception_msgs::msg::PredictedObject obj; obj.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; obj.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; objs.objects.push_back(obj); @@ -425,7 +425,7 @@ TEST_F(EvalTest, TestObstacleTTC) { setTargetMetric(planning_diagnostics::Metric::obstacle_ttc); Objects objs; - autoware_auto_perception_msgs::msg::PredictedObject obj; + autoware_perception_msgs::msg::PredictedObject obj; obj.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; obj.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; objs.objects.push_back(obj); diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index fed268084792a..cf11c65ac805c 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -17,6 +17,7 @@ + @@ -26,6 +27,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index d60f54c2e641a..e1a2260f3ee6d 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -22,6 +22,7 @@ + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 3be8678113728..21143aa0cb28a 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -171,7 +171,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 0e693943a4d03..97bf6414189ab 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -33,7 +33,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 aa649710836da..154a3019f1c17 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 @@ -7,9 +7,9 @@ - + - + @@ -42,13 +42,13 @@ - + - + @@ -220,7 +220,7 @@ - + @@ -242,7 +242,7 @@ - + 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 18de04fd9e317..0612f67de4c2b 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 @@ -2,6 +2,24 @@ + + + + + + + + + + @@ -32,7 +50,7 @@ - + @@ -42,18 +60,18 @@ - + - + - + - + @@ -66,7 +84,7 @@ - + @@ -83,7 +101,7 @@ - + @@ -91,12 +109,44 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + @@ -171,7 +221,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 0a30204ca3c99..be85ee704ff95 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 @@ -21,22 +21,22 @@ - + - - - + + + - + - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index d04a405a61c7b..cf96cd39043ce 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -57,17 +57,17 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner + autoware_path_optimizer autoware_remaining_distance_time_calculator + autoware_velocity_smoother behavior_path_planner - behavior_velocity_planner costmap_generator external_cmd_selector external_velocity_limit_selector freespace_planner glog_component mission_planner - motion_velocity_smoother - obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner planning_evaluator diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index a1bbfc883de7c..6253dce864ddf 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -25,11 +25,11 @@ - - - - - + + + + + @@ -57,6 +57,13 @@ + + + + + + + @@ -70,8 +77,20 @@ + + + + + + + + + + + + - + @@ -85,52 +104,31 @@ - + + - - + + + - - - - - - - - - - + + + - + - - - - - - - - - - - - - - - - diff --git a/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml b/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml index 0395496ae0b78..e2b90b2160a62 100755 --- a/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml +++ b/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 9944cbb84d97c..13e63ddf3c170 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -13,7 +13,7 @@ include_directories( ament_auto_find_build_dependencies() -ament_auto_add_library(ekf_localizer_lib SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/ekf_localizer.cpp src/covariance.cpp src/diagnostics.cpp @@ -24,21 +24,20 @@ ament_auto_add_library(ekf_localizer_lib SHARED src/ekf_module.cpp ) -target_link_libraries(ekf_localizer_lib Eigen3::Eigen) - -ament_auto_add_executable(ekf_localizer src/ekf_localizer_node.cpp) - -target_compile_options(ekf_localizer PUBLIC -g -Wall -Wextra -Wpedantic -Werror) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "EKFLocalizer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) -target_link_libraries(ekf_localizer ekf_localizer_lib) -target_include_directories(ekf_localizer PUBLIC include) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" ekf_localizer_lib) + target_link_libraries("${test_name}" ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 6925e8b63c66b..8ed925c1bc228 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -102,7 +102,7 @@ class Simple1DFilter class EKFLocalizer : public rclcpp::Node { public: - EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit EKFLocalizer(const rclcpp::NodeOptions & options); private: const std::shared_ptr warning_; diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index b6a1bd06185c2..3c66fabe34650 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -17,7 +17,7 @@ - + diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index e9d756e547f26..dad1b0e730711 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -29,6 +29,7 @@ kalman_filter nav_msgs rclcpp + rclcpp_components std_srvs tf2 tf2_ros diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index f77481d45a534..687e812679574 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -40,8 +40,8 @@ using std::placeholders::_1; -EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options), +EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("ekf_localizer", node_options), warning_(std::make_shared(this)), params_(this), ekf_dt_(params_.ekf_dt), @@ -479,3 +479,6 @@ void EKFLocalizer::serviceTriggerNode( res->success = true; return; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(EKFLocalizer) diff --git a/localization/ekf_localizer/src/ekf_localizer_node.cpp b/localization/ekf_localizer/src/ekf_localizer_node.cpp deleted file mode 100644 index 0f75f19c5d50b..0000000000000 --- a/localization/ekf_localizer/src/ekf_localizer_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ekf_localizer/ekf_localizer.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("ekf_localizer", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/localization/geo_pose_projector/CMakeLists.txt b/localization/geo_pose_projector/CMakeLists.txt index 6e2cdf32fb6c5..f274e7d23dbab 100644 --- a/localization/geo_pose_projector/CMakeLists.txt +++ b/localization/geo_pose_projector/CMakeLists.txt @@ -4,11 +4,15 @@ project(geo_pose_projector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(geo_pose_projector - src/geo_pose_projector_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/geo_pose_projector.cpp ) -ament_target_dependencies(geo_pose_projector) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "GeoPoseProjector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml index d840add1ed1c7..de6785a016858 100644 --- a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml +++ b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml index c0e2db59deb64..36b2aec8384ac 100644 --- a/localization/geo_pose_projector/package.xml +++ b/localization/geo_pose_projector/package.xml @@ -24,6 +24,7 @@ geography_utils geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs tf2_ros diff --git a/localization/geo_pose_projector/src/geo_pose_projector.cpp b/localization/geo_pose_projector/src/geo_pose_projector.cpp index d147888bb743b..80bb407958de2 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector.cpp +++ b/localization/geo_pose_projector/src/geo_pose_projector.cpp @@ -25,8 +25,8 @@ #include -GeoPoseProjector::GeoPoseProjector() -: Node("geo_pose_projector"), publish_tf_(declare_parameter("publish_tf")) +GeoPoseProjector::GeoPoseProjector(const rclcpp::NodeOptions & options) +: rclcpp::Node("geo_pose_projector", options), publish_tf_(declare_parameter("publish_tf")) { // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -102,3 +102,6 @@ void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr m tf_broadcaster_->sendTransform(transform_stamped); } } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(GeoPoseProjector) diff --git a/localization/geo_pose_projector/src/geo_pose_projector.hpp b/localization/geo_pose_projector/src/geo_pose_projector.hpp index b0611fec36984..738e805ef8101 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector.hpp +++ b/localization/geo_pose_projector/src/geo_pose_projector.hpp @@ -36,7 +36,7 @@ class GeoPoseProjector : public rclcpp::Node using MapProjectorInfo = map_interface::MapProjectorInfo; public: - GeoPoseProjector(); + explicit GeoPoseProjector(const rclcpp::NodeOptions & options); private: void on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg); diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 57589837dd529..1b142b254d2e1 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -4,17 +4,13 @@ project(gyro_odometer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(${PROJECT_NAME} - src/gyro_odometer_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp + src/diagnostics_module.cpp ) target_link_libraries(${PROJECT_NAME} fmt) -ament_auto_add_library(gyro_odometer_node SHARED - src/gyro_odometer_core.cpp -) - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_gyro_odometer test/test_main.cpp @@ -25,10 +21,15 @@ if(BUILD_TESTING) rclcpp ) target_link_libraries(test_gyro_odometer - gyro_odometer_node + ${PROJECT_NAME} ) endif() +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::gyro_odometer::GyroOdometerNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package(INSTALL_TO_SHARE launch diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index e9e390010f084..aa6f2a96f4838 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -34,3 +34,18 @@ - [Limitation] The frequency of the output messages depends on the frequency of the input IMU message. - [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix. + +## Diagnostics + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| -------------------------------- | ----------------------------------------------------------------------------------------- | ------------------------------- | ------------------------------------------------- | +| `topic_time_stamp` | the time stamp of service calling. [nano second] | none | none | +| `is_arrived_first_vehicle_twist` | whether the vehicle twist topic has been received even once. | not arrive yet | none | +| `is_arrived_first_imu` | whether the imu topic has been received even once. | not arrive yet | none | +| `vehicle_twist_time_stamp_dt` | the time difference between the current time and the latest vehicle twist topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `imu_time_stamp_dt` | the time difference between the current time and the latest imu topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `vehicle_twist_queue_size` | the size of vehicle_twist_queue. | none | none | +| `imu_queue_size` | the size of gyro_queue. | none | none | +| `is_succeed_transform_imu` | whether transform imu is succeed or not. | none | failed | diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp new file mode 100644 index 0000000000000..49b881900b997 --- /dev/null +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -0,0 +1,65 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ +#define GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +class DiagnosticsModule +{ +public: + DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + void clear(); + void addKeyValue(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 publish(const rclcpp::Time & publish_time_stamp); + +private: + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const; + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_; +}; + +template +void DiagnosticsModule::addKeyValue(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); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); + +} // namespace autoware::gyro_odometer + +#endif // GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ 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 3a6358e62c21a..3b2da504f938a 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,6 +15,7 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "gyro_odometer/diagnostics_module.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" @@ -36,19 +37,23 @@ #include #include -class GyroOdometer : public rclcpp::Node +namespace autoware::gyro_odometer +{ + +class GyroOdometerNode : public rclcpp::Node { private: using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - explicit GyroOdometer(const rclcpp::NodeOptions & options); - ~GyroOdometer(); + explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); + ~GyroOdometerNode(); private: void callbackVehicleTwist( 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); rclcpp::Subscription::SharedPtr @@ -71,8 +76,14 @@ class GyroOdometer : public rclcpp::Node bool vehicle_twist_arrived_; bool imu_arrived_; + rclcpp::Time latest_vehicle_twist_ros_time_; + rclcpp::Time latest_imu_ros_time_; std::deque vehicle_twist_queue_; std::deque gyro_queue_; + + std::unique_ptr diagnostics_; }; +} // namespace autoware::gyro_odometer + #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index 21aff3e10d26c..aed6050858fe1 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/localization/gyro_odometer/media/diagnostic.png b/localization/gyro_odometer/media/diagnostic.png new file mode 100644 index 0000000000000..f2324f4079d0d Binary files /dev/null and b/localization/gyro_odometer/media/diagnostic.png differ diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index a0a982ddedc16..575f6a2d74837 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -17,16 +17,16 @@ ament_cmake_auto autoware_cmake + diagnostic_msgs fmt geometry_msgs + rclcpp + rclcpp_components sensor_msgs tf2 tf2_geometry_msgs tier4_autoware_utils - rclcpp - rclcpp_components - ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp new file mode 100644 index 0000000000000..9d88d8e6833ed --- /dev/null +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -0,0 +1,110 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gyro_odometer/diagnostics_module.hpp" + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +: clock_(node->get_clock()) +{ + diagnostics_pub_ = + node->create_publisher("/diagnostics", 10); + + diagnostics_status_msg_.name = + std::string(node->get_name()) + std::string(": ") + diagnostic_name; + diagnostics_status_msg_.hardware_id = node->get_name(); +} + +void DiagnosticsModule::clear() +{ + diagnostics_status_msg_.values.clear(); + diagnostics_status_msg_.values.shrink_to_fit(); + + diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_status_msg_.message = ""; +} + +void DiagnosticsModule::addKeyValue(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), + [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; }); + + if (it != std::cend(diagnostics_status_msg_.values)) { + it->value = key_value_msg.value; + } else { + diagnostics_status_msg_.values.push_back(key_value_msg); + } +} + +template <> +void DiagnosticsModule::addKeyValue(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); +} + +template <> +void DiagnosticsModule::addKeyValue(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); +} + +void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +{ + if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!diagnostics_status_msg_.message.empty()) { + diagnostics_status_msg_.message += "; "; + } + diagnostics_status_msg_.message += message; + } + if (level > diagnostics_status_msg_.level) { + diagnostics_status_msg_.level = level; + } +} + +void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) +{ + diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); +} + +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const +{ + diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; + diagnostics_msg.header.stamp = publish_time_stamp; + diagnostics_msg.status.push_back(diagnostics_status_msg_); + + if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + diagnostics_msg.status.at(0).message = "OK"; + } + + return diagnostics_msg; +} + +} // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 5de0ecd7cdc0a..1852d70bced71 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -14,6 +14,8 @@ #include "gyro_odometer/gyro_odometer_core.hpp" +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -23,8 +25,12 @@ #include #include +#include #include +namespace autoware::gyro_odometer +{ + std::array transformCovariance(const std::array & cov) { using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -39,69 +45,8 @@ std::array transformCovariance(const std::array & cov) return cov_transformed; } -geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( - const std::deque & vehicle_twist_queue, - const std::deque & gyro_queue) -{ - using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; - using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - - double vx_mean = 0; - geometry_msgs::msg::Vector3 gyro_mean{}; - double vx_covariance_original = 0; - geometry_msgs::msg::Vector3 gyro_covariance_original{}; - for (const auto & vehicle_twist : vehicle_twist_queue) { - 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(); - - for (const auto & gyro : gyro_queue) { - gyro_mean.x += gyro.angular_velocity.x; - gyro_mean.y += gyro.angular_velocity.y; - gyro_mean.z += gyro.angular_velocity.z; - gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; - 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(); - - geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; - const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue.back().header.stamp); - const auto latest_imu_stamp = rclcpp::Time(gyro_queue.back().header.stamp); - if (latest_vehicle_twist_stamp < latest_imu_stamp) { - twist_with_cov.header.stamp = latest_imu_stamp; - } else { - twist_with_cov.header.stamp = latest_vehicle_twist_stamp; - } - twist_with_cov.header.frame_id = gyro_queue.front().header.frame_id; - twist_with_cov.twist.twist.linear.x = vx_mean; - twist_with_cov.twist.twist.angular = gyro_mean; - - // 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(); - 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(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue.size(); - - return twist_with_cov; -} - -GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) -: Node("gyro_odometer", options), +GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) +: Node("gyro_odometer", node_options), output_frame_(declare_parameter("output_frame")), message_timeout_sec_(declare_parameter("message_timeout_sec")), vehicle_twist_arrived_(false), @@ -112,10 +57,11 @@ GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); imu_sub_ = create_subscription( - "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1)); + "imu", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); twist_with_covariance_raw_pub_ = create_publisher( @@ -125,123 +71,209 @@ GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); + diagnostics_ = std::make_unique(this, "gyro_odometer_status"); + // TODO(YamatoAndo) createTimer } -GyroOdometer::~GyroOdometer() +GyroOdometerNode::~GyroOdometerNode() { } -void GyroOdometer::callbackVehicleTwist( +void GyroOdometerNode::callbackVehicleTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { + diagnostics_->clear(); + diagnostics_->addKeyValue( + "topic_time_stamp", static_cast(vehicle_twist_ptr->header.stamp).nanoseconds()); + vehicle_twist_arrived_ = true; - if (!imu_arrived_) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed"); + latest_vehicle_twist_ros_time_ = vehicle_twist_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + concatGyroAndOdometer(); + + diagnostics_->publish(vehicle_twist_ptr->header.stamp); +} + +void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +{ + diagnostics_->clear(); + diagnostics_->addKeyValue( + "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(); + + diagnostics_->publish(imu_msg_ptr->header.stamp); +} + +void GyroOdometerNode::concatGyroAndOdometer() +{ + // check arrive first topic + diagnostics_->addKeyValue("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->addKeyValue("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( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (!imu_arrived_) { + std::stringstream message; + message << "Imu msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); - const double twist_dt = std::abs((this->now() - vehicle_twist_ptr->header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + // check timeout + 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); + 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); - if (gyro_queue_.empty()) return; - const double imu_dt = std::abs((this->now() - gyro_queue_.back().header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (imu_dt > message_timeout_sec_) { + 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); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); -} - -void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) -{ - imu_arrived_ = true; - if (!vehicle_twist_arrived_) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed"); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - const double imu_dt = std::abs((this->now() - imu_msg_ptr->header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); + // check queue size + diagnostics_->addKeyValue("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->addKeyValue("imu_queue_size", gyro_queue_.size()); + if (vehicle_twist_queue_.empty()) { + // not output error and clear queue + return; + } + if (gyro_queue_.empty()) { + // not output error and clear queue return; } + // get transformation geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = - transform_listener_->getLatestTransform(imu_msg_ptr->header.frame_id, output_frame_); - if (!tf_imu2base_ptr) { - RCLCPP_ERROR( - this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), - (imu_msg_ptr->header.frame_id).c_str()); + 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); + 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( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.header = imu_msg_ptr->header; - angular_velocity.vector = imu_msg_ptr->angular_velocity; - - geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - transformed_angular_velocity.header = tf_imu2base_ptr->header; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); - - sensor_msgs::msg::Imu gyro_base_link; - gyro_base_link.header = imu_msg_ptr->header; - gyro_base_link.header.frame_id = output_frame_; - gyro_base_link.angular_velocity = transformed_angular_velocity.vector; - gyro_base_link.angular_velocity_covariance = - transformCovariance(imu_msg_ptr->angular_velocity_covariance); - - gyro_queue_.push_back(gyro_base_link); - - if (vehicle_twist_queue_.empty()) return; - const double twist_dt = - std::abs((this->now() - vehicle_twist_queue_.back().header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; + // transform gyro frame + for (auto & gyro : gyro_queue_) { + geometry_msgs::msg::Vector3Stamped angular_velocity; + angular_velocity.header = gyro.header; + angular_velocity.vector = gyro.angular_velocity; + + geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; + transformed_angular_velocity.header = tf_imu2base_ptr->header; + tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); + + gyro.header.frame_id = output_frame_; + gyro.angular_velocity = transformed_angular_velocity.vector; + gyro.angular_velocity_covariance = transformCovariance(gyro.angular_velocity_covariance); } - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); + using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + + // calc mean, covariance + double vx_mean = 0; + geometry_msgs::msg::Vector3 gyro_mean{}; + double vx_covariance_original = 0; + geometry_msgs::msg::Vector3 gyro_covariance_original{}; + for (const auto & vehicle_twist : vehicle_twist_queue_) { + 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(); + + for (const auto & gyro : gyro_queue_) { + gyro_mean.x += gyro.angular_velocity.x; + gyro_mean.y += gyro.angular_velocity.y; + gyro_mean.z += gyro.angular_velocity.z; + gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; + 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(); + + // concat + geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; + const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue_.back().header.stamp); + const auto latest_imu_stamp = rclcpp::Time(gyro_queue_.back().header.stamp); + if (latest_vehicle_twist_stamp < latest_imu_stamp) { + twist_with_cov.header.stamp = latest_imu_stamp; + } else { + twist_with_cov.header.stamp = latest_vehicle_twist_stamp; + } + twist_with_cov.header.frame_id = gyro_queue_.front().header.frame_id; + twist_with_cov.twist.twist.linear.x = vx_mean; + twist_with_cov.twist.twist.angular = gyro_mean; + + // 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(); + 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(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = + gyro_covariance_original.y / gyro_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = + gyro_covariance_original.z / gyro_queue_.size(); + + publishData(twist_with_cov); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); } -void GyroOdometer::publishData( +void GyroOdometerNode::publishData( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; @@ -269,3 +301,8 @@ void GyroOdometer::publishData( twist_pub_->publish(twist); twist_with_covariance_pub_->publish(twist_with_covariance); } + +} // namespace autoware::gyro_odometer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode) diff --git a/localization/gyro_odometer/src/gyro_odometer_node.cpp b/localization/gyro_odometer/src/gyro_odometer_node.cpp deleted file mode 100644 index 5bb6241506fbe..0000000000000 --- a/localization/gyro_odometer/src/gyro_odometer_node.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "gyro_odometer/gyro_odometer_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto node = std::make_shared(options); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index 7f6416fbdda16..b7849ef03bfc5 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -108,7 +108,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(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -135,7 +136,8 @@ TEST(GyroOdometer, TestGyroOdometerImuOnly) { Imu input_imu = generateSampleImu(); - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt index d625064b8f6cb..a8435fa056847 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt @@ -14,27 +14,33 @@ ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) -ament_auto_add_executable(ar_tag_based_localizer - src/main.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/ar_tag_based_localizer.cpp ) -target_include_directories(ar_tag_based_localizer + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS} ) -target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "ArTagBasedLocalizer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_ar_tag_based_localizer + ament_auto_add_gtest(test_${PROJECT_NAME} test/test.cpp src/ar_tag_based_localizer.cpp ) - target_include_directories(test_ar_tag_based_localizer + target_include_directories(test_${PROJECT_NAME} SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS} ) - target_link_libraries(test_ar_tag_based_localizer ${OpenCV_LIBRARIES}) + target_link_libraries(test_${PROJECT_NAME} ${OpenCV_LIBRARIES}) endif() ament_auto_package( diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md index 1a776bd810fff..37596ee9820b9 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -15,7 +15,7 @@ The positions and orientations of the AR-Tags are assumed to be written in the L | Name | Type | Description | | :--------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | +| `~/input/lanelet2_map` | `autoware_map_msgs::msg::LaneletMapBin` | Data of lanelet2 | | `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | | `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | | `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 272338905c3f0..34602ca70daf4 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 072479cc7aaf5..90fdd2fee31f4 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -24,6 +24,7 @@ lanelet2_extension localization_util rclcpp + rclcpp_components tf2_eigen tf2_geometry_msgs tf2_ros diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 43ac1e1098453..0ea40690d9f4d 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -65,7 +65,7 @@ #include ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) -: Node("ar_tag_based_localizer", options), cam_info_received_(false) +: rclcpp::Node("ar_tag_based_localizer", options), cam_info_received_(false) { /* Declare node parameters @@ -111,7 +111,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) Subscribers */ using std::placeholders::_1; - map_bin_sub_ = this->create_subscription( + map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1)); @@ -140,7 +140,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); } -void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) +void ArTagBasedLocalizer::map_bin_callback(const LaneletMapBin::ConstSharedPtr & msg) { landmark_manager_.parse_landmarks(msg, "apriltag_16h5"); const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); @@ -346,3 +346,6 @@ std::vector ArTagBasedLocalizer::detect_landmarks( return landmarks; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ArTagBasedLocalizer) diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index f70821a39ffe8..f00621b3c520a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -70,7 +70,7 @@ class ArTagBasedLocalizer : public rclcpp::Node { - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using Image = sensor_msgs::msg::Image; using CameraInfo = sensor_msgs::msg::CameraInfo; using Pose = geometry_msgs::msg::Pose; @@ -86,7 +86,7 @@ class ArTagBasedLocalizer : public rclcpp::Node explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: - void map_bin_callback(const HADMapBin::ConstSharedPtr & msg); + void map_bin_callback(const LaneletMapBin::ConstSharedPtr & msg); void image_callback(const Image::ConstSharedPtr & msg); void cam_info_callback(const CameraInfo::ConstSharedPtr & msg); void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg); @@ -106,7 +106,7 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr tf_listener_; // Subscribers - rclcpp::Subscription::SharedPtr map_bin_sub_; + rclcpp::Subscription::SharedPtr map_bin_sub_; rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr cam_info_sub_; rclcpp::Subscription::SharedPtr ekf_pose_sub_; diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp deleted file mode 100644 index 8ef1dd6195580..0000000000000 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// This source code is derived from the https://github.com/pal-robotics/aruco_ros. -// Here is the license statement. -/***************************** - Copyright 2011 Rafael Muñoz Salinas. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, are - permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this list of - conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, this list - of conditions and the following disclaimer in the documentation and/or other materials - provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED - WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR - CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - The views and conclusions contained in the software and documentation are those of the - authors and should not be interpreted as representing official policies, either expressed - or implied, of Rafael Muñoz Salinas. - ********************************/ - -#include "ar_tag_based_localizer.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - std::shared_ptr ptr = std::make_shared(); - rclcpp::spin(ptr); - rclcpp::shutdown(); -} diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index c2b0751d91f9a..7c1a3f656dd66 100644 --- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp @@ -19,7 +19,7 @@ #include -#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" #include #include #include @@ -41,7 +41,7 @@ class LandmarkManager { public: void parse_landmarks( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg, const std::string & target_subtype); [[nodiscard]] std::vector get_landmarks() const; diff --git a/localization/landmark_based_localizer/landmark_manager/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml index be19de9334e84..4e080c0362c07 100644 --- a/localization/landmark_based_localizer/landmark_manager/package.xml +++ b/localization/landmark_based_localizer/landmark_manager/package.xml @@ -18,7 +18,7 @@ eigen - autoware_auto_mapping_msgs + autoware_map_msgs geometry_msgs lanelet2_extension localization_util diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index 57bfcde461af6..a808a6428f682 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -28,7 +28,7 @@ namespace landmark_manager { void LandmarkManager::parse_landmarks( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg, const std::string & target_subtype) { std::vector landmarks = @@ -50,7 +50,7 @@ void LandmarkManager::parse_landmarks( const auto & v2 = vertices[2]; const auto & v3 = vertices[3]; const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; - const double volume_threshold = 1e-5; + const double volume_threshold = 1e-3; if (volume > volume_threshold) { continue; } diff --git a/localization/localization_error_monitor/CMakeLists.txt b/localization/localization_error_monitor/CMakeLists.txt index 3528367486350..c27e51e6e0359 100644 --- a/localization/localization_error_monitor/CMakeLists.txt +++ b/localization/localization_error_monitor/CMakeLists.txt @@ -4,15 +4,16 @@ project(localization_error_monitor) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(localization_error_monitor_module SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/diagnostics.cpp + src/localization_error_monitor.cpp ) -ament_auto_add_executable(localization_error_monitor - src/main.cpp - src/node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "LocalizationErrorMonitor" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor ) -target_link_libraries(localization_error_monitor localization_error_monitor_module) if(BUILD_TESTING) function(add_testcase filepath) @@ -20,17 +21,17 @@ if(BUILD_TESTING) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - target_link_libraries(${test_name} localization_error_monitor_module) + target_link_libraries(${test_name} ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() - find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() add_testcase(test/test_diagnostics.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE config launch ) diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp similarity index 87% rename from localization/localization_error_monitor/include/localization_error_monitor/node.hpp rename to localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp index 296b278004333..6016677d88136 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_ERROR_MONITOR__NODE_HPP_ -#define LOCALIZATION_ERROR_MONITOR__NODE_HPP_ +#ifndef LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ +#define LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ #include #include @@ -58,7 +58,7 @@ class LocalizationErrorMonitor : public rclcpp::Node double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); public: - LocalizationErrorMonitor(); + explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options); ~LocalizationErrorMonitor() = default; }; -#endif // LOCALIZATION_ERROR_MONITOR__NODE_HPP_ +#endif // LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ diff --git a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml index eb2b5741b00fc..ad3e8beff92ab 100644 --- a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml +++ b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index a1b352d911a0d..0138451b069e4 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -23,6 +23,7 @@ diagnostic_msgs diagnostic_updater nav_msgs + rclcpp_components tf2 tf2_geometry_msgs tier4_autoware_utils diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp similarity index 93% rename from localization/localization_error_monitor/src/node.cpp rename to localization/localization_error_monitor/src/localization_error_monitor.cpp index f47372a0158b2..7a1cd0b213c39 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_error_monitor/node.hpp" +#include "localization_error_monitor/localization_error_monitor.hpp" #include "localization_error_monitor/diagnostics.hpp" @@ -32,7 +32,8 @@ #include #include -LocalizationErrorMonitor::LocalizationErrorMonitor() : Node("localization_error_monitor") +LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & options) +: Node("localization_error_monitor", options) { scale_ = this->declare_parameter("scale"); error_ellipse_size_ = this->declare_parameter("error_ellipse_size"); @@ -128,7 +129,7 @@ void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr in diag_merged_status.hardware_id = this->get_name(); diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = this->now(); + diag_msg.header.stamp = input_msg->header.stamp; diag_msg.status.push_back(diag_merged_status); diag_pub_->publish(diag_msg); } @@ -143,3 +144,6 @@ double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( double d = std::sqrt((e.transpose() * Pinv * e)(0, 0) / Pinv.determinant()); return d; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(LocalizationErrorMonitor) diff --git a/localization/localization_error_monitor/src/main.cpp b/localization/localization_error_monitor/src/main.cpp deleted file mode 100644 index f4fa5ab664005..0000000000000 --- a/localization/localization_error_monitor/src/main.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "localization_error_monitor/node.hpp" - -#include - -#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/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 7ac78514c49fe..5e4db9571c404 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -22,20 +22,24 @@ else() endif() endif() -find_package(glog REQUIRED) find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) -ament_auto_add_executable(ndt_scan_matcher +ament_auto_add_library(${PROJECT_NAME} SHARED src/diagnostics_module.cpp src/map_update_module.cpp src/ndt_scan_matcher_core.cpp - src/ndt_scan_matcher_node.cpp src/particle.cpp ) link_directories(${PCL_LIBRARY_DIRS}) -target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES} glog::glog) +target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "NDTScanMatcher" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) if(BUILD_TESTING) add_launch_test( @@ -46,19 +50,9 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_auto_add_gtest(standard_sequence_for_initial_pose_estimation test/test_cases/standard_sequence_for_initial_pose_estimation.cpp - src/diagnostics_module.cpp - src/map_update_module.cpp - src/ndt_scan_matcher_core.cpp -# src/ndt_scan_matcher_node.cpp - src/particle.cpp ) ament_auto_add_gtest(once_initialize_at_out_of_map_then_initialize_correctly test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp - src/diagnostics_module.cpp - src/map_update_module.cpp - src/ndt_scan_matcher_core.cpp -# src/ndt_scan_matcher_node.cpp - src/particle.cpp ) endif() diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index fbc2fa3c9a3f3..a44db7bbaa4bf 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -262,23 +262,23 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num drawing -| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | -| ----------------------------------------- | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -------------------------------- | ------------------------------------------------------------------------ | -| `topic_time_stamp` | the time stamp of input topic | none | none | no | -| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | -| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | -| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | -| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | -| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | yes | -| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | -| `is_set_map_points` | whether the map points is set or not | not set | none | yes | -| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | -| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | -| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | -| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | -| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | -| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | -| `skipping_publish_num` | the number of times rejected estimation results consecutively | none | the number of times is 5 or more | - | +| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | +| ----------------------------------------- | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- | +| `topic_time_stamp` | the time stamp of input topic | none | none | no | +| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | +| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | +| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | +| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | +| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. | +| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | +| `is_set_map_points` | whether the map points is set or not | not set | none | yes | +| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | +| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | +| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | +| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | +| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | +| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | +| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - | ※The `sensor_points_callback` shares the same callback group as the `trigger_node_service` and `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale. diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp index 828eb6bed346b..4f7b5eff6521b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp @@ -31,10 +31,11 @@ class DiagnosticsModule template void addKeyValue(const std::string & key, const T & value); void updateLevelAndMessage(const int8_t level, const std::string & message); - void publish(); + void publish(const rclcpp::Time & publish_time_stamp); private: - diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray() const; + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const; rclcpp::Clock::SharedPtr clock_; rclcpp::Publisher::SharedPtr diagnostics_pub_; diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index cbcb0a9f74bc4..671e2ace56cad 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index b9a1f7dbad1bd..b62e810926331 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -22,13 +22,13 @@ diagnostic_msgs fmt geometry_msgs - glog libpcl-all-dev localization_util nav_msgs ndt_omp pcl_conversions rclcpp + rclcpp_components sensor_msgs std_msgs std_srvs diff --git a/localization/ndt_scan_matcher/src/diagnostics_module.cpp b/localization/ndt_scan_matcher/src/diagnostics_module.cpp index 9d7eed46414ea..1e08ebb89f51e 100644 --- a/localization/ndt_scan_matcher/src/diagnostics_module.cpp +++ b/localization/ndt_scan_matcher/src/diagnostics_module.cpp @@ -85,15 +85,16 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str } } -void DiagnosticsModule::publish() +void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) { - diagnostics_pub_->publish(createDiagnosticsArray()); + diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray() const +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; - diagnostics_msg.header.stamp = clock_->now(); + diagnostics_msg.header.stamp = publish_time_stamp; diagnostics_msg.status.push_back(diagnostics_status_msg_); if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 43174a39b797e..40f0b1f4465fa 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -52,8 +52,6 @@ void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().seconds()); - // check is_activated diagnostics_ptr->addKeyValue("is_activated", is_activated); if (!is_activated) { diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 8692027e9634a..8acfe3bd5c1ca 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -193,11 +193,15 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) void NDTScanMatcher::callback_timer() { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_map_update_->clear(); + diagnostics_map_update_->addKeyValue("timer_callback_time_stamp", ros_time_now.nanoseconds()); + map_update_module_->callback_timer(is_activated_, latest_ekf_position_, diagnostics_map_update_); - diagnostics_map_update_->publish(); + diagnostics_map_update_->publish(ros_time_now); } void NDTScanMatcher::callback_initial_pose( @@ -207,14 +211,15 @@ void NDTScanMatcher::callback_initial_pose( callback_initial_pose_main(initial_pose_msg_ptr); - diagnostics_initial_pose_->publish(); + diagnostics_initial_pose_->publish(initial_pose_msg_ptr->header.stamp); } void NDTScanMatcher::callback_initial_pose_main( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { diagnostics_initial_pose_->addKeyValue( - "topic_time_stamp", static_cast(initial_pose_msg_ptr->header.stamp).seconds()); + "topic_time_stamp", + static_cast(initial_pose_msg_ptr->header.stamp).nanoseconds()); // check is_activated diagnostics_initial_pose_->addKeyValue("is_activated", static_cast(is_activated_)); @@ -255,11 +260,11 @@ void NDTScanMatcher::callback_regularization_pose( diagnostics_regularization_pose_->clear(); diagnostics_regularization_pose_->addKeyValue( - "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).seconds()); + "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).nanoseconds()); regularization_pose_buffer_->push_back(pose_conv_msg_ptr); - diagnostics_regularization_pose_->publish(); + diagnostics_regularization_pose_->publish(pose_conv_msg_ptr->header.stamp); } void NDTScanMatcher::callback_sensor_points( @@ -275,16 +280,17 @@ void NDTScanMatcher::callback_sensor_points( // check skipping_publish_num static size_t skipping_publish_num = 0; const size_t error_skipping_publish_num = 5; - skipping_publish_num = is_succeed_scan_matching ? 0 : (skipping_publish_num + 1); + skipping_publish_num = + ((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1)); diagnostics_scan_points_->addKeyValue("skipping_publish_num", skipping_publish_num); if (skipping_publish_num >= error_skipping_publish_num) { std::stringstream message; message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times)."; diagnostics_scan_points_->updateLevelAndMessage( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } - diagnostics_scan_points_->publish(); + diagnostics_scan_points_->publish(sensor_points_msg_in_sensor_frame->header.stamp); } bool NDTScanMatcher::callback_sensor_points_main( @@ -294,7 +300,7 @@ bool NDTScanMatcher::callback_sensor_points_main( // check topic_time_stamp const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; - diagnostics_scan_points_->addKeyValue("topic_time_stamp", sensor_ros_time.seconds()); + diagnostics_scan_points_->addKeyValue("topic_time_stamp", sensor_ros_time.nanoseconds()); // check sensor_points_size const size_t sensor_points_size = sensor_points_msg_in_sensor_frame->width; @@ -865,8 +871,10 @@ void NDTScanMatcher::service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_trigger_node_->clear(); - diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", this->now().seconds()); + diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", ros_time_now.nanoseconds()); is_activated_ = req->data; if (is_activated_) { @@ -876,15 +884,19 @@ void NDTScanMatcher::service_trigger_node( diagnostics_trigger_node_->addKeyValue("is_activated", static_cast(is_activated_)); diagnostics_trigger_node_->addKeyValue("is_succeed_service", res->success); - diagnostics_trigger_node_->publish(); + diagnostics_trigger_node_->publish(ros_time_now); } void NDTScanMatcher::service_ndt_align( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_ndt_align_->clear(); + diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", ros_time_now.nanoseconds()); + service_ndt_align_main(req, res); // check is_succeed_service @@ -897,15 +909,13 @@ void NDTScanMatcher::service_ndt_align( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } - diagnostics_ndt_align_->publish(); + diagnostics_ndt_align_->publish(ros_time_now); } void NDTScanMatcher::service_ndt_align_main( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { - diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().seconds()); - // get TF from pose_frame to map_frame const std::string & target_frame = param_.frame.map_frame; const std::string & source_frame = req->pose_with_covariance.header.frame_id; @@ -1069,3 +1079,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( return result_pose_with_cov_msg; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(NDTScanMatcher) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp deleted file mode 100644 index 785055eef3700..0000000000000 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); // NOLINT - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - auto ndt_scan_matcher = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor exec; - exec.add_node(ndt_scan_matcher); - exec.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index a2fbbddf7d120..2a586aa9cd049 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/localization/pose2twist/CMakeLists.txt @@ -4,11 +4,16 @@ project(pose2twist) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose2twist - src/pose2twist_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose2twist_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Pose2Twist" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index c2a39f7e2ed3d..459a62ea5cd13 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -24,7 +24,7 @@ class Pose2Twist : public rclcpp::Node { public: - Pose2Twist(); + explicit Pose2Twist(const rclcpp::NodeOptions & options); ~Pose2Twist() = default; private: diff --git a/localization/pose2twist/launch/pose2twist.launch.xml b/localization/pose2twist/launch/pose2twist.launch.xml index a0fae57a163e6..57a41dbfcf017 100644 --- a/localization/pose2twist/launch/pose2twist.launch.xml +++ b/localization/pose2twist/launch/pose2twist.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index 16c49bb51c218..07e445c72978c 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -19,6 +19,7 @@ geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs tier4_debug_msgs diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index a321a06122816..4b98ec6c81ad4 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -24,7 +24,7 @@ #include #include -Pose2Twist::Pose2Twist() : Node("pose2twist_core") +Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose2twist", options) { using std::placeholders::_1; @@ -129,3 +129,6 @@ void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_ms angular_z_msg.data = twist_msg.twist.angular.z; angular_z_pub_->publish(angular_z_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Pose2Twist) diff --git a/localization/pose2twist/src/pose2twist_node.cpp b/localization/pose2twist/src/pose2twist_node.cpp deleted file mode 100644 index 81f98461ecffd..0000000000000 --- a/localization/pose2twist/src/pose2twist_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "pose2twist/pose2twist_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/pose_estimator_arbiter/CMakeLists.txt b/localization/pose_estimator_arbiter/CMakeLists.txt index 9a47b654a6ab4..eefb7fc9a6879 100644 --- a/localization/pose_estimator_arbiter/CMakeLists.txt +++ b/localization/pose_estimator_arbiter/CMakeLists.txt @@ -4,29 +4,23 @@ project(pose_estimator_arbiter) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(glog REQUIRED) - find_package(PCL REQUIRED COMPONENTS common) include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) -# ============================== -# switch rule library -ament_auto_add_library(switch_rule - SHARED - src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp -) -target_include_directories(switch_rule PUBLIC src) - # ============================== # pose estimator arbiter node -ament_auto_add_executable(${PROJECT_NAME} +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp - src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp + src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC src) -target_link_libraries(${PROJECT_NAME} switch_rule glog::glog) -# ============================== +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "pose_estimator_arbiter::PoseEstimatorArbiter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -53,6 +47,7 @@ endif() add_subdirectory(example_rule) # ============================== -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch ) diff --git a/localization/pose_estimator_arbiter/README.md b/localization/pose_estimator_arbiter/README.md index 7e1ad2b9ec6eb..a28da699ad926 100644 --- a/localization/pose_estimator_arbiter/README.md +++ b/localization/pose_estimator_arbiter/README.md @@ -93,7 +93,7 @@ For switching rule: | Name | Type | Description | | ----------------------------- | ------------------------------------------------------------ | --------------------------------- | -| `/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | | `/input/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | localization final output | | `/input/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | localization initialization state | diff --git a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt index 333f92842b860..b2b5a828e42e7 100644 --- a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt +++ b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt @@ -7,7 +7,7 @@ ament_auto_add_library(example_rule src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp ) target_include_directories(example_rule PUBLIC src example_rule/src) -target_link_libraries(example_rule switch_rule) +target_link_libraries(example_rule pose_estimator_arbiter) # ============================== # define test definition macro @@ -16,7 +16,7 @@ function(add_testcase filepath) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" switch_rule example_rule) + target_link_libraries("${test_name}" pose_estimator_arbiter example_rule) target_include_directories(${test_name} PUBLIC src) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp index d6901f9be2dbf..392219b2281e1 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -29,7 +29,7 @@ namespace pose_estimator_arbiter::rule_helper { class PoseEstimatorArea { - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp index 094434c62ac9c..675842662acf8 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp @@ -27,7 +27,7 @@ VectorMapBasedRule::VectorMapBasedRule( // Register callback shared_data_->vector_map.register_callback( - [this](autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) -> void { + [this](autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) -> void { pose_estimator_area_->init(msg); }); diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp index 35ed8b04bfcad..beda4d720b7f6 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp +++ b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp @@ -18,7 +18,7 @@ #include -#include +#include #include @@ -59,7 +59,7 @@ TEST_F(MockNode, poseEstimatorArea) lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet_map->add(create_polygon3d()); - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using Point = geometry_msgs::msg::Point; HADMapBin msg; lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp index a0a983e2ad3b7..66d867883272b 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp @@ -16,7 +16,7 @@ #include -#include +#include #include #include @@ -71,7 +71,7 @@ TEST_F(MockNode, vectorMapBasedRule) lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet_map->add(create_polygon3d()); - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; HADMapBin msg; lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); diff --git a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml index 0a708e3f48988..b5be96fc3ce44 100644 --- a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml +++ b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml index 480b323f3031d..70896d93d9cb8 100644 --- a/localization/pose_estimator_arbiter/package.xml +++ b/localization/pose_estimator_arbiter/package.xml @@ -12,15 +12,15 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_mapping_msgs + autoware_map_msgs diagnostic_msgs geometry_msgs - glog lanelet2_extension magic_enum pcl_conversions pcl_ros pluginlib + rclcpp_components sensor_msgs std_msgs std_srvs @@ -29,9 +29,6 @@ visualization_msgs yabloc_particle_filter - rclcpp - rclcpp_components - ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp index 9e67dfc063964..fda77ec877482 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp @@ -42,12 +42,12 @@ class PoseEstimatorArbiter : public rclcpp::Node using Image = sensor_msgs::msg::Image; using PointCloud2 = sensor_msgs::msg::PointCloud2; using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; public: - PoseEstimatorArbiter(); + explicit PoseEstimatorArbiter(const rclcpp::NodeOptions & options); private: // Set of running pose estimators specified by ros param `pose_sources` diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp index 4fc3fd9b914a6..67c555227976d 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp @@ -42,8 +42,8 @@ static std::unordered_set parse_estimator_name_args( return running_estimator_list; } -PoseEstimatorArbiter::PoseEstimatorArbiter() -: Node("pose_estimator_arbiter"), +PoseEstimatorArbiter::PoseEstimatorArbiter(const rclcpp::NodeOptions & options) +: rclcpp::Node("pose_estimator_arbiter", options), running_estimator_list_(parse_estimator_name_args( declare_parameter>("pose_sources"), get_logger())), logger_configure_(std::make_unique(this)) @@ -211,3 +211,6 @@ void PoseEstimatorArbiter::on_timer() } } // namespace pose_estimator_arbiter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pose_estimator_arbiter::PoseEstimatorArbiter) diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp deleted file mode 100644 index 20aaaf10abaab..0000000000000 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "pose_estimator_arbiter/pose_estimator_arbiter.hpp" - -#include - -int main(int argc, char * argv[]) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp index b9c3bd45c9e24..a72dc5585877b 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp @@ -16,7 +16,7 @@ #define POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ #include -#include +#include #include #include #include @@ -77,7 +77,7 @@ struct SharedData using Image = sensor_msgs::msg::Image; using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using PointCloud2 = sensor_msgs::msg::PointCloud2; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; SharedData() {} diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt index 81a60a1d6b717..08d5bb47fca09 100644 --- a/localization/pose_initializer/CMakeLists.txt +++ b/localization/pose_initializer/CMakeLists.txt @@ -4,8 +4,7 @@ project(pose_initializer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose_initializer_node - src/pose_initializer/pose_initializer_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_initializer/pose_initializer_core.cpp src/pose_initializer/gnss_module.cpp src/pose_initializer/ndt_module.cpp @@ -15,6 +14,12 @@ ament_auto_add_executable(pose_initializer_node src/pose_initializer/ndt_localization_trigger_module.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "PoseInitializer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + if(BUILD_TESTING) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) @@ -30,7 +35,8 @@ if(BUILD_TESTING) add_testcase(test/test_copy_vector_to_array.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch config ) diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 7006becf00c2f..36cc92e1d235b 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -17,9 +17,9 @@ This node depends on the map height fitter library. ### Services -| Name | Type | Description | -| -------------------------- | --------------------------------------------------- | --------------------- | -| `/localization/initialize` | autoware_adapi_v1_msgs::srv::InitializeLocalization | initial pose from api | +| Name | Type | Description | +| -------------------------- | ---------------------------------------------------- | --------------------- | +| `/localization/initialize` | tier4_localization_msgs::srv::InitializeLocalization | initial pose from api | ### Clients @@ -46,3 +46,86 @@ This node depends on the map height fitter library. This `pose_initializer` is used via default AD API. For detailed description of the API description, please refer to [the description of `default_ad_api`](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/localization.md). drawing + +## Initialize pose via CLI + +### Using the GNSS estimated position + +```bash +ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization +``` + +The GNSS estimated position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position. + +### Using the input position + +```bash +ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization " +pose_with_covariance: + - header: + frame_id: map + pose: + pose: + position: + x: 89571.1640625 + y: 42301.1875 + z: -3.1565165519714355 + orientation: + x: 0.0 + y: 0.0 + z: 0.28072773940524687 + w: 0.9597874433062874 + covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467] +method: 0 +" +``` + +The input position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position. + +### Direct initial position set + +```bash +ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization " +pose_with_covariance: + - header: + frame_id: map + pose: + pose: + position: + x: 89571.1640625 + y: 42301.1875 + z: -3.1565165519714355 + orientation: + x: 0.0 + y: 0.0 + z: 0.28072773940524687 + w: 0.9597874433062874 + covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467] +method: 1 +" +``` + +The initial position is set directly by the input position without going through localization algorithm. + +### Via ros2 topic pub + +```bash +ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped " +header: + frame_id: map +pose: + pose: + position: + x: 89571.1640625 + y: 42301.1875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.28072773940524687 + w: 0.9597874433062874 +" +``` + +It behaves the same as "initialpose (from rviz)". +The position.z and the covariance will be overwritten by [ad_api_adaptors](https://github.com/autowarefoundation/autoware.universe/tree/main/system/default_ad_api_helpers/ad_api_adaptors), so there is no need to input them. diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 3e4911e2c2936..2ffdebf39c474 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 85c9c26bd6c8c..58968828bc2fa 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -25,6 +25,7 @@ map_height_fitter motion_utils rclcpp + rclcpp_components std_srvs tier4_autoware_utils tier4_localization_msgs diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index bc86b5476dcca..2fc7d61890837 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -23,9 +23,11 @@ #include "yabloc_module.hpp" #include +#include #include -PoseInitializer::PoseInitializer() : Node("pose_initializer") +PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) +: rclcpp::Node("pose_initializer", options) { const auto node = component_interface_utils::NodeAdaptor(this); group_srv_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -79,7 +81,7 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer") initial_pose.orientation.z = initial_pose_array[5]; initial_pose.orientation.w = initial_pose_array[6]; - set_user_defined_initial_pose(initial_pose); + set_user_defined_initial_pose(initial_pose, true); } } } @@ -114,11 +116,12 @@ void PoseInitializer::change_node_trigger(bool flag, bool need_spin) } } -void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose) +void PoseInitializer::set_user_defined_initial_pose( + const geometry_msgs::msg::Pose initial_pose, bool need_spin) { try { change_state(State::Message::INITIALIZING); - change_node_trigger(false, true); + change_node_trigger(false, need_spin); PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; @@ -127,7 +130,7 @@ void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Po pose.pose.covariance = output_pose_covariance_; pub_reset_->publish(pose); - change_node_trigger(true, true); + change_node_trigger(true, need_spin); change_state(State::Message::INITIALIZED); RCLCPP_INFO(get_logger(), "Set user defined initial pose"); @@ -147,25 +150,52 @@ void PoseInitializer::on_initialize( Initialize::Service::Response::ERROR_UNSAFE, "The vehicle is not stopped."); } try { - change_state(State::Message::INITIALIZING); - change_node_trigger(false, false); - - auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front(); - if (ndt_) { - pose = ndt_->align_pose(pose); - } else if (yabloc_) { - // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more - // accuracy pose. - pose = yabloc_->align_pose(pose); - } - pose.pose.covariance = output_pose_covariance_; - pub_reset_->publish(pose); + if (req->method == Initialize::Service::Request::AUTO) { + change_state(State::Message::INITIALIZING); + change_node_trigger(false, false); + + auto pose = + req->pose_with_covariance.empty() ? get_gnss_pose() : req->pose_with_covariance.front(); + if (ndt_) { + pose = ndt_->align_pose(pose); + } else if (yabloc_) { + // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more + // accuracy pose. + pose = yabloc_->align_pose(pose); + } + pose.pose.covariance = output_pose_covariance_; + pub_reset_->publish(pose); + + change_node_trigger(true, false); + res->status.success = true; + change_state(State::Message::INITIALIZED); + + } else if (req->method == Initialize::Service::Request::DIRECT) { + if (req->pose_with_covariance.empty()) { + std::stringstream message; + message << "No input pose_with_covariance. If you want to use DIRECT method, please input " + "pose_with_covariance."; + RCLCPP_ERROR(get_logger(), message.str().c_str()); + throw ServiceException( + autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); + } + auto pose = req->pose_with_covariance.front().pose.pose; + set_user_defined_initial_pose(pose, false); + res->status.success = true; - change_node_trigger(true, false); - res->status.success = true; - change_state(State::Message::INITIALIZED); + } else { + std::stringstream message; + message << "Unknown method type (=" << std::to_string(req->method) << ")"; + RCLCPP_ERROR(get_logger(), message.str().c_str()); + throw ServiceException( + autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); + } } catch (const ServiceException & error) { - res->status = error.status(); + autoware_adapi_v1_msgs::msg::ResponseStatus respose_status; + respose_status = error.status(); + res->status.success = respose_status.success; + res->status.code = respose_status.code; + res->status.message = respose_status.message; change_state(State::Message::UNINITIALIZED); } } @@ -180,3 +210,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseInitializer::get_gnss_pose() throw ServiceException( Initialize::Service::Response::ERROR_GNSS_SUPPORT, "GNSS is not supported."); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(PoseInitializer) diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index 623cfe50307f5..eeba71b90129b 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -34,7 +34,7 @@ class NdtLocalizationTriggerModule; class PoseInitializer : public rclcpp::Node { public: - PoseInitializer(); + explicit PoseInitializer(const rclcpp::NodeOptions & options); ~PoseInitializer(); private: @@ -60,7 +60,8 @@ class PoseInitializer : public rclcpp::Node double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); - void set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose); + void set_user_defined_initial_pose( + const geometry_msgs::msg::Pose initial_pose, bool need_spin = false); void change_state(State::Message::_state_type state); void on_initialize( const Initialize::Service::Request::SharedPtr req, diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp deleted file mode 100644 index c5b31c4e37ccd..0000000000000 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2020 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "pose_initializer_core.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/pose_instability_detector/CMakeLists.txt index 5270df636d791..c6f94ab7df16e 100644 --- a/localization/pose_instability_detector/CMakeLists.txt +++ b/localization/pose_instability_detector/CMakeLists.txt @@ -4,14 +4,19 @@ project(pose_instability_detector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose_instability_detector - src/main.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_instability_detector.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "PoseInstabilityDetector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_pose_instability_detector + ament_auto_add_gtest(test_${PROJECT_NAME} test/test.cpp src/pose_instability_detector.cpp ) @@ -19,6 +24,6 @@ endif() ament_auto_package( INSTALL_TO_SHARE - launch - config + launch + config ) diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml index 4a390ee2854d7..5ebe7d7e429e0 100644 --- a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml +++ b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index bf57e5589b747..d658d1c2d437f 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -22,6 +22,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components tf2 tf2_geometry_msgs tier4_autoware_utils diff --git a/localization/pose_instability_detector/src/main.cpp b/localization/pose_instability_detector/src/main.cpp deleted file mode 100644 index 34e679e86730f..0000000000000 --- a/localization/pose_instability_detector/src/main.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023- Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "pose_instability_detector.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto pose_instability_detector = std::make_shared(); - rclcpp::spin(pose_instability_detector); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index afb7d6e007db2..c2bce6a3db288 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -23,7 +23,7 @@ #include PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) -: Node("pose_instability_detector", 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")), @@ -174,3 +174,6 @@ void PoseInstabilityDetector::callback_timer() prev_odometry_ = latest_odometry_; twist_buffer_.clear(); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(PoseInstabilityDetector) diff --git a/localization/stop_filter/CMakeLists.txt b/localization/stop_filter/CMakeLists.txt index 97a0443195ae5..4776a1f4967b2 100644 --- a/localization/stop_filter/CMakeLists.txt +++ b/localization/stop_filter/CMakeLists.txt @@ -4,11 +4,15 @@ project(stop_filter) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(stop_filter - src/stop_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/stop_filter.cpp ) -ament_target_dependencies(stop_filter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "StopFilter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/stop_filter/include/stop_filter/stop_filter.hpp b/localization/stop_filter/include/stop_filter/stop_filter.hpp index 3c2b91590234f..24145a7920d91 100644 --- a/localization/stop_filter/include/stop_filter/stop_filter.hpp +++ b/localization/stop_filter/include/stop_filter/stop_filter.hpp @@ -37,7 +37,7 @@ class StopFilter : public rclcpp::Node { public: - StopFilter(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit StopFilter(const rclcpp::NodeOptions & options); private: rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher diff --git a/localization/stop_filter/launch/stop_filter.launch.xml b/localization/stop_filter/launch/stop_filter.launch.xml index 0ea92d26c9677..c53b37efc9954 100644 --- a/localization/stop_filter/launch/stop_filter.launch.xml +++ b/localization/stop_filter/launch/stop_filter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index 83eaf1b9fa821..2e6d5c4f8bbe1 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -21,6 +21,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components tf2 tier4_debug_msgs diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index ac0960b8cb67b..4d6b2c6240867 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -24,8 +24,8 @@ using std::placeholders::_1; -StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) +StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("stop_filter", node_options) { vx_threshold_ = declare_parameter("vx_threshold"); wz_threshold_ = declare_parameter("wz_threshold"); @@ -57,3 +57,6 @@ void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) pub_stop_flag_->publish(stop_flag_msg); pub_odom_->publish(odom_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(StopFilter) diff --git a/localization/stop_filter/src/stop_filter_node.cpp b/localization/stop_filter/src/stop_filter_node.cpp deleted file mode 100644 index 9748214828de2..0000000000000 --- a/localization/stop_filter/src/stop_filter_node.cpp +++ /dev/null @@ -1,28 +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 "stop_filter/stop_filter.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("stop_filter", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/localization/twist2accel/CMakeLists.txt b/localization/twist2accel/CMakeLists.txt index 59f23eacb2fb3..9178bf02288a8 100644 --- a/localization/twist2accel/CMakeLists.txt +++ b/localization/twist2accel/CMakeLists.txt @@ -4,11 +4,15 @@ project(twist2accel) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(twist2accel - src/twist2accel_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/twist2accel.cpp ) -ament_target_dependencies(twist2accel) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Twist2Accel" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/twist2accel/include/twist2accel/twist2accel.hpp b/localization/twist2accel/include/twist2accel/twist2accel.hpp index da5f2b4a3b228..0db69890fbfe8 100644 --- a/localization/twist2accel/include/twist2accel/twist2accel.hpp +++ b/localization/twist2accel/include/twist2accel/twist2accel.hpp @@ -40,7 +40,7 @@ class Twist2Accel : public rclcpp::Node { public: - Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit Twist2Accel(const rclcpp::NodeOptions & options); private: rclcpp::Publisher::SharedPtr diff --git a/localization/twist2accel/launch/twist2accel.launch.xml b/localization/twist2accel/launch/twist2accel.launch.xml index c4c9a3ed50bfc..c534a288aac3e 100644 --- a/localization/twist2accel/launch/twist2accel.launch.xml +++ b/localization/twist2accel/launch/twist2accel.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index 08dacf9185769..0dbce08f309ac 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -21,6 +21,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components signal_processing tf2 tier4_debug_msgs diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/twist2accel/src/twist2accel.cpp index 68019f27e95fe..0af29445bbeb8 100644 --- a/localization/twist2accel/src/twist2accel.cpp +++ b/localization/twist2accel/src/twist2accel.cpp @@ -24,8 +24,8 @@ using std::placeholders::_1; -Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) +Twist2Accel::Twist2Accel(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("twist2accel", node_options) { sub_odom_ = create_subscription( "input/odom", 1, std::bind(&Twist2Accel::callbackOdometry, this, _1)); @@ -103,3 +103,6 @@ void Twist2Accel::estimateAccel(const geometry_msgs::msg::TwistStamped::SharedPt pub_accel_->publish(accel_msg); prev_twist_ptr_ = msg; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Twist2Accel) diff --git a/localization/twist2accel/src/twist2accel_node.cpp b/localization/twist2accel/src/twist2accel_node.cpp deleted file mode 100644 index ce8ed31db5c32..0000000000000 --- a/localization/twist2accel/src/twist2accel_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2022 TIER IV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "twist2accel/twist2accel.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("twist2accel", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/localization/yabloc/yabloc_common/CMakeLists.txt b/localization/yabloc/yabloc_common/CMakeLists.txt index c8474a85ad963..ca2f28bae7cda 100644 --- a/localization/yabloc/yabloc_common/CMakeLists.txt +++ b/localization/yabloc/yabloc_common/CMakeLists.txt @@ -23,9 +23,6 @@ find_package(Sophus REQUIRED) # because it rewrite CMAKE_NO_SYSTEM_FROM_IMPORTED to TRUE. set(CMAKE_NO_SYSTEM_FROM_IMPORTED FALSE) -# glog -find_package(glog REQUIRED) - # =================================================== # GeographicLib find_package(PkgConfig) @@ -45,7 +42,10 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/static_tf_subscriber.cpp src/extract_line_segments.cpp src/transform_line_segments.cpp - src/color.cpp) + src/color.cpp + src/ground_server/ground_server_core.cpp + src/ground_server/polygon_operation.cpp + src/ll2_decomposer/ll2_decomposer_core.cpp) target_include_directories( ${PROJECT_NAME} PUBLIC include ) @@ -63,23 +63,18 @@ target_link_libraries(${PROJECT_NAME} Geographic ${PCL_LIBRARIES} Sophus::Sophus # =================================================== # Executables # ground_server -set(TARGET ground_server_node) -ament_auto_add_executable(${TARGET} - src/ground_server/ground_server_core.cpp - src/ground_server/ground_server_node.cpp - src/ground_server/polygon_operation.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus glog::glog) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::ground_server::GroundServer" + EXECUTABLE yabloc_ground_server_node + EXECUTOR SingleThreadedExecutor +) # ll2_decomposer -set(TARGET ll2_decomposer_node) -ament_auto_add_executable(${TARGET} - src/ll2_decomposer/ll2_decomposer_core.cpp - src/ll2_decomposer/ll2_decomposer_node.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::ll2_decomposer::Ll2Decomposer" + EXECUTABLE yabloc_ll2_decomposer_node + EXECUTOR SingleThreadedExecutor +) # =================================================== ament_export_dependencies(PCL Sophus) diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md index 6368305bdbfad..cb1799ce21e61 100644 --- a/localization/yabloc/yabloc_common/README.md +++ b/localization/yabloc/yabloc_common/README.md @@ -15,10 +15,10 @@ It estimates the height and tilt of the ground from lanelet2. #### Input -| Name | Type | Description | -| ------------------ | -------------------------------------------- | ------------------- | -| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | -| `input/pose` | `geometry_msgs::msg::PoseStamped` | estimated self pose | +| Name | Type | Description | +| ------------------ | --------------------------------------- | ------------------- | +| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | +| `input/pose` | `geometry_msgs::msg::PoseStamped` | estimated self pose | #### Output @@ -44,9 +44,9 @@ This node extracts the elements related to the road surface markings and yabloc #### Input -| Name | Type | Description | -| ------------------ | -------------------------------------------- | ----------- | -| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| Name | Type | Description | +| ------------------ | --------------------------------------- | ----------- | +| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | #### Output diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp index 154507f8195d9..92e5d939d0b7e 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include @@ -44,7 +44,7 @@ class GroundServer : public rclcpp::Node { public: using GroundPlane = common::GroundPlane; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; @@ -57,7 +57,7 @@ class GroundServer : public rclcpp::Node using String = std_msgs::msg::String; using PointCloud2 = sensor_msgs::msg::PointCloud2; using Point = geometry_msgs::msg::Point; - GroundServer(); + explicit GroundServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const bool force_zero_tilt_; @@ -65,7 +65,7 @@ class GroundServer : public rclcpp::Node const int K; // Subscriber - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_pose_stamped_; rclcpp::Subscription::SharedPtr sub_initial_pose_; // Publisher @@ -86,7 +86,7 @@ class GroundServer : public rclcpp::Node std::vector last_indices_; // Callback - void on_map(const HADMapBin & msg); + void on_map(const LaneletMapBin & msg); void on_initial_pose(const PoseCovStamped & msg); void on_pose_stamped(const PoseStamped & msg); diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp index f6e6de6b38fdd..b044ae985c6c0 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -35,11 +35,11 @@ class Ll2Decomposer : public rclcpp::Node { public: using Cloud2 = sensor_msgs::msg::PointCloud2; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; - Ll2Decomposer(); + explicit Ll2Decomposer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: rclcpp::Publisher::SharedPtr pub_road_marking_; @@ -47,12 +47,12 @@ class Ll2Decomposer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_bounding_box_; rclcpp::Publisher::SharedPtr pub_marker_; - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_map_; std::set road_marking_labels_; std::set sign_board_labels_; std::set bounding_box_labels_; - void on_map(const HADMapBin & msg); + void on_map(const LaneletMapBin & msg); pcl::PointNormal to_point_normal( const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) const; diff --git a/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml b/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml index 48299e880cc9c..29d3da71bce7f 100644 --- a/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml +++ b/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml @@ -1,5 +1,7 @@ + + @@ -7,7 +9,7 @@ - + @@ -25,7 +27,7 @@ - + diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index 213057f428c38..de675da5cc2d4 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -16,14 +16,14 @@ autoware_cmake rosidl_default_generators - autoware_auto_mapping_msgs + autoware_map_msgs cv_bridge geometry_msgs - glog lanelet2_core lanelet2_extension pcl_conversions rclcpp + rclcpp_components sensor_msgs signal_processing sophus diff --git a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp index 9987c4fbb5e72..9940dca1fd62b 100644 --- a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp +++ b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp @@ -20,12 +20,13 @@ pcl::PointCloud extract_near_line_segments( const pcl::PointCloud & line_segments, const Sophus::SE3f & transform, const float max_range) { - constexpr double sqrt_two = std::sqrt(2); + constexpr double sqrt_two = 1.41421356237309504880; const Eigen::Vector3f pose_vector = transform.translation(); // All line segments contained in a square with max_range on one side must be taken out, // so pick up those that are closer than the **diagonals** of the square. - auto check_intersection = [max_range, pose_vector](const pcl::PointNormal & pn) -> bool { + auto check_intersection = [sqrt_two, max_range, + pose_vector](const pcl::PointNormal & pn) -> bool { const Eigen::Vector3f from = pn.getVector3fMap() - pose_vector; const Eigen::Vector3f to = pn.getNormalVector3fMap() - pose_vector; diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp index 749aa39318923..d2d1929991442 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp @@ -30,8 +30,8 @@ namespace yabloc::ground_server { -GroundServer::GroundServer() -: Node("ground_server"), +GroundServer::GroundServer(const rclcpp::NodeOptions & options) +: Node("ground_server", options), force_zero_tilt_(declare_parameter("force_zero_tilt")), R(declare_parameter("R")), K(declare_parameter("K")) @@ -43,7 +43,7 @@ GroundServer::GroundServer() auto on_pose = std::bind(&GroundServer::on_pose_stamped, this, _1); auto on_map = std::bind(&GroundServer::on_map, this, _1); - sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); + sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); sub_pose_stamped_ = create_subscription("~/input/pose", 10, on_pose); pub_ground_height_ = create_publisher("~/output/height", 10); @@ -100,7 +100,7 @@ void GroundServer::on_pose_stamped(const PoseStamped & msg) } } -void GroundServer::on_map(const HADMapBin & msg) +void GroundServer::on_map(const LaneletMapBin & msg) { lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(msg, lanelet_map); @@ -248,3 +248,6 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point) } } // namespace yabloc::ground_server + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ground_server::GroundServer) diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp deleted file mode 100644 index 4e032ff72c998..0000000000000 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp +++ /dev/null @@ -1,29 +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 "yabloc_common/ground_server/ground_server.hpp" - -#include - -int main(int argc, char ** argv) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); -} 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 5cf3e905e447d..d0dfc87067f7f 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 @@ -24,7 +24,7 @@ namespace yabloc::ll2_decomposer { -Ll2Decomposer::Ll2Decomposer() : Node("ll2_to_image") +Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to_image", options) { using std::placeholders::_1; const rclcpp::QoS latch_qos = rclcpp::QoS(10).transient_local(); @@ -38,7 +38,7 @@ Ll2Decomposer::Ll2Decomposer() : Node("ll2_to_image") // Subscriber auto cb_map = std::bind(&Ll2Decomposer::on_map, this, _1); - sub_map_ = create_subscription("~/input/vector_map", map_qos, cb_map); + sub_map_ = create_subscription("~/input/vector_map", map_qos, cb_map); auto load_lanelet2_labels = [this](const std::string & param_name, std::set & labels) -> void { @@ -102,7 +102,7 @@ pcl::PointCloud Ll2Decomposer::load_bounding_boxes( return cloud; } -void Ll2Decomposer::on_map(const HADMapBin & msg) +void Ll2Decomposer::on_map(const LaneletMapBin & msg) { RCLCPP_INFO_STREAM(get_logger(), "subscribed binary vector map"); lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); @@ -263,3 +263,6 @@ void Ll2Decomposer::publish_additional_marker(const lanelet::LaneletMapPtr & lan } } // namespace yabloc::ll2_decomposer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ll2_decomposer::Ll2Decomposer) diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp deleted file mode 100644 index 692f6a5da0913..0000000000000 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp +++ /dev/null @@ -1,23 +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 "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/CMakeLists.txt b/localization/yabloc/yabloc_image_processing/CMakeLists.txt index 58437c085c4b2..2af75ab237585 100644 --- a/localization/yabloc/yabloc_image_processing/CMakeLists.txt +++ b/localization/yabloc/yabloc_image_processing/CMakeLists.txt @@ -14,56 +14,55 @@ find_package(OpenCV REQUIRED) # PCL find_package(PCL REQUIRED COMPONENTS common) +ament_auto_add_library(${PROJECT_NAME} SHARED + src/line_segment_detector/line_segment_detector_core.cpp + src/graph_segment/graph_segment_core.cpp + src/graph_segment/similar_area_searcher.cpp + src/segment_filter/segment_filter_core.cpp + src/undistort/undistort_node.cpp + src/line_segments_overlay/line_segments_overlay_core.cpp + src/lanelet2_overlay/lanelet2_overlay_core.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ${OpenCV_LIBS}) + # =================================================== # Executable -# line segment detector -set(TARGET line_segment_detector_node) -ament_auto_add_executable(${TARGET} - src/line_segment_detector/line_segment_detector_node.cpp - src/line_segment_detector/line_segment_detector_core.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::graph_segment::GraphSegment" + EXECUTABLE yabloc_graph_segment_node + EXECUTOR SingleThreadedExecutor +) -# graph based segmentation -set(TARGET graph_segment_node) -ament_auto_add_executable(${TARGET} - src/graph_segment/graph_segment_node.cpp - src/graph_segment/graph_segment_core.cpp - src/graph_segment/similar_area_searcher.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::lanelet2_overlay::Lanelet2Overlay" + EXECUTABLE yabloc_lanelet2_overlay_node + EXECUTOR SingleThreadedExecutor +) -# segment filter -set(TARGET segment_filter_node) -ament_auto_add_executable(${TARGET} - src/segment_filter/segment_filter_node.cpp - src/segment_filter/segment_filter_core.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::line_segment_detector::LineSegmentDetector" + EXECUTABLE yabloc_line_segment_detector_node + EXECUTOR SingleThreadedExecutor +) -# undistort -set(TARGET undistort_node) -ament_auto_add_executable(${TARGET} - src/undistort/undistort_node.cpp) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::line_segments_overlay::LineSegmentsOverlay" + EXECUTABLE yabloc_line_segments_overlay_node + EXECUTOR SingleThreadedExecutor +) -# line_segments_overlay -set(TARGET line_segments_overlay_node) -ament_auto_add_executable(${TARGET} - src/line_segments_overlay/line_segments_overlay_core.cpp - src/line_segments_overlay/line_segments_overlay_node.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::segment_filter::SegmentFilter" + EXECUTABLE yabloc_segment_filter_node + EXECUTOR SingleThreadedExecutor +) -# lanelet2_overlay -set(TARGET lanelet2_overlay_node) -ament_auto_add_executable(${TARGET} - src/lanelet2_overlay/lanelet2_overlay_core.cpp - src/lanelet2_overlay/lanelet2_overlay_node.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::undistort::UndistortNode" + EXECUTABLE yabloc_undistort_node + EXECUTOR SingleThreadedExecutor +) # =================================================== ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp index a1e9c90eaa362..6c2e670a7a72e 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp @@ -32,7 +32,7 @@ class GraphSegment : public rclcpp::Node public: using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - GraphSegment(); + explicit GraphSegment(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const float target_height_ratio_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp index 468e765b74175..7d644376ba591 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp @@ -47,7 +47,7 @@ class Lanelet2Overlay : public rclcpp::Node using Image = sensor_msgs::msg::Image; using Float32Array = std_msgs::msg::Float32MultiArray; - Lanelet2Overlay(); + explicit Lanelet2Overlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: common::StaticTfSubscriber tf_subscriber_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp index 74ef82dd94f59..761c581200369 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp @@ -42,7 +42,7 @@ class LineSegmentDetector : public rclcpp::Node using Image = sensor_msgs::msg::Image; using PointCloud2 = sensor_msgs::msg::PointCloud2; - LineSegmentDetector(); + explicit LineSegmentDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: rclcpp::Subscription::SharedPtr sub_image_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp index abbc2f75725ed..00f01be5984e5 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp @@ -34,7 +34,7 @@ class LineSegmentsOverlay : public rclcpp::Node using Image = sensor_msgs::msg::Image; using LineSegment = pcl::PointXYZLNormal; using LineSegments = pcl::PointCloud; - LineSegmentsOverlay(); + explicit LineSegmentsOverlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: void on_image(const Image::ConstSharedPtr & img_msg); diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp index 766bb77d4da85..c8f036f2b12de 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp @@ -39,7 +39,7 @@ class SegmentFilter : public rclcpp::Node using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - SegmentFilter(); + explicit SegmentFilter(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: using ProjectFunc = std::function(const Eigen::Vector3f &)>; diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index a0cad16302c2b..214772751f931 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -5,7 +5,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -27,7 +27,7 @@ - + @@ -40,7 +40,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml index 150ffed58138d..134f3ee765476 100644 --- a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml @@ -10,7 +10,7 @@ - + @@ -23,7 +23,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 209f09fdaa7ac..416acfdc76a16 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -19,6 +19,7 @@ cv_bridge pcl_conversions rclcpp + rclcpp_components sensor_msgs std_msgs tier4_autoware_utils diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp index 75e14b5a4cd4b..f8d4c74dd5bf8 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::graph_segment { -GraphSegment::GraphSegment() -: Node("graph_segment"), +GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) +: Node("graph_segment", options), target_height_ratio_(declare_parameter("target_height_ratio")), target_candidate_box_width_(declare_parameter("target_candidate_box_width")) { @@ -159,3 +159,6 @@ void GraphSegment::draw_and_publish_image( } } // namespace yabloc::graph_segment + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::graph_segment::GraphSegment) diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp deleted file mode 100644 index d11701e115eff..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp +++ /dev/null @@ -1,23 +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 "yabloc_image_processing/graph_segment/graph_segment.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp index 367ff51567147..107d861364038 100644 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp @@ -28,8 +28,8 @@ namespace yabloc::lanelet2_overlay { -Lanelet2Overlay::Lanelet2Overlay() -: Node("lanelet2_overlay"), tf_subscriber_(get_clock()), pose_buffer_{40} +Lanelet2Overlay::Lanelet2Overlay(const rclcpp::NodeOptions & options) +: Node("lanelet2_overlay", options), tf_subscriber_(get_clock()), pose_buffer_{40} { using std::placeholders::_1; @@ -211,3 +211,6 @@ void Lanelet2Overlay::make_vis_marker( } } // namespace yabloc::lanelet2_overlay + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::lanelet2_overlay::Lanelet2Overlay) diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp deleted file mode 100644 index 941ac9d84ab16..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp +++ /dev/null @@ -1,23 +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 "yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index 57f2302fd5c5a..c613642628499 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -23,7 +23,8 @@ namespace yabloc::line_segment_detector { -LineSegmentDetector::LineSegmentDetector() : Node("line_detector") +LineSegmentDetector::LineSegmentDetector(const rclcpp::NodeOptions & options) +: Node("line_detector", options) { using std::placeholders::_1; @@ -106,3 +107,6 @@ std::vector LineSegmentDetector::remove_too_outer_elements( } } // namespace yabloc::line_segment_detector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segment_detector::LineSegmentDetector) diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp deleted file mode 100644 index 42965ae853ea7..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp +++ /dev/null @@ -1,23 +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 "yabloc_image_processing/line_segment_detector/line_segment_detector.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp index 0ee4115a39760..7e06de81fbd18 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::line_segments_overlay { -LineSegmentsOverlay::LineSegmentsOverlay() -: Node("overlay_lanelet2"), +LineSegmentsOverlay::LineSegmentsOverlay(const rclcpp::NodeOptions & options) +: Node("line_segments_overlay", options), max_buffer_size_(static_cast(declare_parameter("max_buffer_size", 5))) { using std::placeholders::_1; @@ -90,3 +90,6 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l } } // namespace yabloc::line_segments_overlay + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segments_overlay::LineSegmentsOverlay) diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp deleted file mode 100644 index cac6a6a0c5a66..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp +++ /dev/null @@ -1,23 +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 "yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp index cb1505903b210..df0aa7d65c617 100644 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::segment_filter { -SegmentFilter::SegmentFilter() -: Node("segment_filter"), +SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) +: Node("segment_filter", options), image_size_(declare_parameter("image_size")), max_range_(declare_parameter("max_range")), min_segment_length_(declare_parameter("min_segment_length")), @@ -282,3 +282,6 @@ std::set SegmentFilter::filter_by_mask( } } // namespace yabloc::segment_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::segment_filter::SegmentFilter) diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp deleted file mode 100644 index ea51babceee60..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp +++ /dev/null @@ -1,23 +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 "yabloc_image_processing/segment_filter/segment_filter.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 7fc9ad785dbe2..0714b6c8091c8 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -37,8 +37,8 @@ class UndistortNode : public rclcpp::Node using CameraInfo = sensor_msgs::msg::CameraInfo; using Image = sensor_msgs::msg::Image; - UndistortNode() - : Node("undistort"), + explicit UndistortNode(const rclcpp::NodeOptions & options) + : Node("undistort", options), OUTPUT_WIDTH(declare_parameter("width")), OVERRIDE_FRAME_ID(declare_parameter("override_frame_id")) { @@ -166,10 +166,5 @@ class UndistortNode : public rclcpp::Node }; } // namespace yabloc::undistort -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::undistort::UndistortNode) diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 5db4aa0c29e88..66c0d8af8865c 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -18,6 +18,7 @@ rosidl_default_generators geometry_msgs + glog rclcpp sensor_msgs sophus diff --git a/localization/yabloc/yabloc_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md index 33b9788cca3d3..9ab5dd570510d 100644 --- a/localization/yabloc/yabloc_pose_initializer/README.md +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -45,11 +45,11 @@ Converted model URL #### Input -| Name | Type | Description | -| ------------------- | -------------------------------------------- | ------------------------ | -| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info | -| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image | -| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| Name | Type | Description | +| ------------------- | --------------------------------------- | ------------------------ | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info | +| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image | +| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | #### Output diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp index 917a6ecaaf291..979d1b370699e 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include @@ -40,7 +40,7 @@ class CameraPoseInitializer : public rclcpp::Node using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped; CameraPoseInitializer(); @@ -52,7 +52,7 @@ class CameraPoseInitializer : public rclcpp::Node std::unique_ptr projector_module_{nullptr}; rclcpp::Subscription::SharedPtr sub_initialpose_; - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_image_; rclcpp::Service::SharedPtr align_server_; @@ -63,7 +63,7 @@ class CameraPoseInitializer : public rclcpp::Node std::unique_ptr semantic_segmentation_{nullptr}; - void on_map(const HADMapBin & msg); + void on_map(const LaneletMapBin & msg); void on_service( const RequestPoseAlignment::Request::SharedPtr, RequestPoseAlignment::Response::SharedPtr request); diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index afd0d4aa86bcf..7ed16c9a8b82d 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -16,7 +16,7 @@ autoware_cmake rosidl_default_generators - autoware_auto_mapping_msgs + autoware_map_msgs cv_bridge geometry_msgs lanelet2_extension diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp index 19d3c8d88f260..083e7dd5bcd43 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -40,7 +40,7 @@ CameraPoseInitializer::CameraPoseInitializer() // Subscriber auto on_map = std::bind(&CameraPoseInitializer::on_map, this, _1); auto on_image = [this](Image::ConstSharedPtr msg) -> void { latest_image_msg_ = msg; }; - sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); + sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); sub_image_ = create_subscription("~/input/image_raw", 10, on_image); // Server @@ -164,7 +164,7 @@ std::optional CameraPoseInitializer::estimate_pose( return angles_rad.at(max_index); } -void CameraPoseInitializer::on_map(const HADMapBin & msg) +void CameraPoseInitializer::on_map(const LaneletMapBin & msg) { lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(msg, lanelet_map); diff --git a/map/map_height_fitter/CMakeLists.txt b/map/map_height_fitter/CMakeLists.txt index 8821158c54757..0dec2f6a1663a 100644 --- a/map/map_height_fitter/CMakeLists.txt +++ b/map/map_height_fitter/CMakeLists.txt @@ -5,8 +5,9 @@ find_package(autoware_cmake REQUIRED) find_package(PCL REQUIRED COMPONENTS common) autoware_package() -ament_auto_add_library(map_height_fitter SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/map_height_fitter.cpp + src/map_height_fitter_node.cpp ) target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) @@ -14,11 +15,14 @@ target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) # These are treated as errors in compile, so pedantic warnings are disabled for this package. target_compile_options(map_height_fitter PRIVATE -Wno-pedantic) -ament_auto_add_executable(node - src/node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "MapHeightFitterNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor ) ament_auto_package( INSTALL_TO_SHARE launch - config) + config +) diff --git a/map/map_height_fitter/launch/map_height_fitter.launch.xml b/map/map_height_fitter/launch/map_height_fitter.launch.xml index 353f2151ee172..3e01a35a8e519 100644 --- a/map/map_height_fitter/launch/map_height_fitter.launch.xml +++ b/map/map_height_fitter/launch/map_height_fitter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 7f384aa43ec7b..c10e65cdfaab1 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -18,13 +18,13 @@ ament_cmake autoware_cmake - autoware_auto_mapping_msgs autoware_map_msgs geometry_msgs lanelet2_extension libpcl-common pcl_conversions rclcpp + rclcpp_components sensor_msgs tf2_geometry_msgs tf2_ros diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 0c99d33772aea..e01201f90f7e3 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -38,7 +38,7 @@ struct MapHeightFitter::Impl explicit Impl(rclcpp::Node * node); void on_pcd_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void on_vector_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_vector_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); bool get_partial_point_cloud_map(const Point & point); double get_ground_height(const Point & point) const; std::optional fit(const Point & position, const std::string & frame); @@ -59,7 +59,7 @@ struct MapHeightFitter::Impl // for fitting by vector_map_loader lanelet::LaneletMapPtr vector_map_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; }; MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), node_(node) @@ -95,7 +95,7 @@ MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), n } else if (fit_target_ == "vector_map") { const auto durable_qos = rclcpp::QoS(1).transient_local(); - sub_vector_map_ = node_->create_subscription( + sub_vector_map_ = node_->create_subscription( "~/vector_map", durable_qos, std::bind(&MapHeightFitter::Impl::on_vector_map, this, std::placeholders::_1)); @@ -163,7 +163,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } void MapHeightFitter::Impl::on_vector_map( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { vector_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, vector_map_); diff --git a/map/map_height_fitter/src/node.cpp b/map/map_height_fitter/src/map_height_fitter_node.cpp similarity index 83% rename from map/map_height_fitter/src/node.cpp rename to map/map_height_fitter/src/map_height_fitter_node.cpp index 55702dc08450d..fdc7604b68855 100644 --- a/map/map_height_fitter/src/node.cpp +++ b/map/map_height_fitter/src/map_height_fitter_node.cpp @@ -23,7 +23,8 @@ using tier4_localization_msgs::srv::PoseWithCovarianceStamped; class MapHeightFitterNode : public rclcpp::Node { public: - MapHeightFitterNode() : Node("map_height_fitter"), fitter_(this) + explicit MapHeightFitterNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("map_height_fitter", options), fitter_(this) { const auto on_service = [this]( const PoseWithCovarianceStamped::Request::SharedPtr req, @@ -46,13 +47,5 @@ class MapHeightFitterNode : public rclcpp::Node rclcpp::Service::SharedPtr srv_; }; -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapHeightFitterNode) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 8a31ecee50be5..21bd39303250f 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -130,7 +130,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Feature -lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. +lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_map_msgs/LaneletMapBin message. The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. @@ -144,7 +144,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Published Topics -- ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map +- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map ### Parameters @@ -156,7 +156,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. +lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray. ### How to Run @@ -164,7 +164,7 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa ### Subscribed Topics -- ~input/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : binary data of Lanelet2 Map +- ~input/lanelet2_map (autoware_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map ### Published Topics diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 0adc7612e9f61..4e85ddec056c1 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; using tier4_map_msgs::msg::MapProjectorInfo; class Lanelet2MapLoaderNode : public rclcpp::Node @@ -38,7 +38,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info); - static HADMapBin create_map_bin_msg( + static LaneletMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); @@ -48,7 +48,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; - rclcpp::Publisher::SharedPtr pub_map_bin_; + rclcpp::Publisher::SharedPtr pub_map_bin_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp index e5a5fe3e3a6fa..cb640e4dc83d5 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -29,12 +29,12 @@ class Lanelet2MapVisualizationNode : public rclcpp::Node explicit Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_map_bin_; + rclcpp::Subscription::SharedPtr sub_map_bin_; rclcpp::Publisher::SharedPtr pub_marker_; bool viz_lanelets_centerline_; - void onMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); }; #endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml index ea0157620ce43..5baee911d6572 100644 --- a/map/map_loader/launch/lanelet2_map_loader.launch.xml +++ b/map/map_loader/launch/lanelet2_map_loader.launch.xml @@ -4,16 +4,16 @@ - + - + - + diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index 3e447456bb623..3633a8fa39a6b 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index a9b657f843447..4eaf8600dcab4 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -19,7 +19,6 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs autoware_map_msgs component_interface_specs component_interface_utils diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 617f3dd503ce0..b097e1809a385 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -86,7 +86,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( // create publisher and publish pub_map_bin_ = - create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); + create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published."); } @@ -141,18 +141,18 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return nullptr; } -HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( +LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { std::string format_version{}, map_version{}; lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); - HADMapBin map_bin_msg; + LaneletMapBin map_bin_msg; map_bin_msg.header.stamp = now; map_bin_msg.header.frame_id = "map"; - map_bin_msg.format_version = format_version; - map_bin_msg.map_version = map_version; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); return map_bin_msg; diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index bdfbcf904cf36..c81236bec86c0 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include @@ -73,7 +73,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt viz_lanelets_centerline_ = true; - sub_map_bin_ = this->create_subscription( + sub_map_bin_ = this->create_subscription( "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1)); @@ -82,7 +82,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt } void Lanelet2MapVisualizationNode::onMapBin( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index f6102a1efa795..87f519ab22572 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -6,25 +6,24 @@ autoware_package() ament_auto_find_build_dependencies() -ament_auto_add_library(map_projection_loader_lib SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/map_projection_loader.cpp src/load_info_from_lanelet2_map.cpp ) -target_link_libraries(map_projection_loader_lib yaml-cpp) - -ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp) - -target_compile_options(map_projection_loader PUBLIC -g -Wall -Wextra -Wpedantic -Werror) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "MapProjectionLoader" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) -target_link_libraries(map_projection_loader map_projection_loader_lib) -target_include_directories(map_projection_loader PUBLIC include) +target_link_libraries(${PROJECT_NAME} yaml-cpp) 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}" map_projection_loader_lib) + target_link_libraries("${test_name}" ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() @@ -57,7 +56,8 @@ if(BUILD_TESTING) add_testcase(test/test_load_info_from_lanelet2_map.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch config ) diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 54e794e2742bf..05bc6e64e1675 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -29,7 +29,7 @@ tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( class MapProjectionLoader : public rclcpp::Node { public: - MapProjectionLoader(); + explicit MapProjectionLoader(const rclcpp::NodeOptions & options); private: using MapProjectorInfo = map_interface::MapProjectorInfo; diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index a6570b69d3498..13418a7e97423 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 475881577bd58..7a930085cd7b1 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -21,6 +21,7 @@ component_interface_utils lanelet2_extension rclcpp + rclcpp_components tier4_map_msgs yaml-cpp diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 5966baaed8383..383051e8f67a5 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -82,7 +82,8 @@ tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( return msg; } -MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +MapProjectionLoader::MapProjectionLoader(const rclcpp::NodeOptions & options) +: rclcpp::Node("map_projection_loader", options) { const std::string yaml_filename = this->declare_parameter("map_projector_info_path"); const std::string lanelet2_map_filename = @@ -96,3 +97,6 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") adaptor.init_pub(publisher_); publisher_->publish(msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapProjectionLoader) diff --git a/map/map_projection_loader/src/map_projection_loader_node.cpp b/map/map_projection_loader/src/map_projection_loader_node.cpp deleted file mode 100644 index 1d9336be0d9dd..0000000000000 --- a/map/map_projection_loader/src/map_projection_loader_node.cpp +++ /dev/null @@ -1,23 +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 "map_projection_loader/map_projection_loader.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py index b8540550ce9da..0d0b5cb31afba 100644 --- a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py index c7697038cc253..6a17ff340b19f 100644 --- a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index f75beddc6827c..37cfd9936bf20 100644 --- a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py index 765f3cde04679..7bccdc7875454 100644 --- a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_tf_generator/README.md b/map/map_tf_generator/README.md index 643f4233c06f0..eef36c34750ca 100644 --- a/map/map_tf_generator/README.md +++ b/map/map_tf_generator/README.md @@ -25,9 +25,9 @@ The following are the supported methods to calculate the position of the `viewer #### vector_map_tf_generator -| Name | Type | Description | -| ----------------- | -------------------------------------------- | ------------------------------------------------------------- | -| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Subscribe vector map to calculate position of `viewer` frames | +| Name | Type | Description | +| ----------------- | --------------------------------------- | ------------------------------------------------------------- | +| `/map/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | Subscribe vector map to calculate position of `viewer` frames | ### Output diff --git a/map/map_tf_generator/launch/map_tf_generator.launch.xml b/map/map_tf_generator/launch/map_tf_generator.launch.xml index 43d487c2bc982..197085f31d9c4 100644 --- a/map/map_tf_generator/launch/map_tf_generator.launch.xml +++ b/map/map_tf_generator/launch/map_tf_generator.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp index 20ca1c037a578..f242254a456a1 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node map_frame_ = declare_parameter("map_frame"); viewer_frame_ = declare_parameter("viewer_frame"); - sub_ = create_subscription( + sub_ = create_subscription( "vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapTFGeneratorNode::onVectorMap, this, std::placeholders::_1)); @@ -44,12 +44,12 @@ class VectorMapTFGeneratorNode : public rclcpp::Node private: std::string map_frame_; std::string viewer_frame_; - rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr sub_; std::shared_ptr static_broadcaster_; std::shared_ptr lanelet_map_ptr_; - void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + void onVectorMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); diff --git a/perception/bytetrack/package.xml b/perception/bytetrack/package.xml index 0d1ccc284f888..231a1845bb0ea 100644 --- a/perception/bytetrack/package.xml +++ b/perception/bytetrack/package.xml @@ -15,7 +15,7 @@ cudnn_cmake_module tensorrt_cmake_module - autoware_auto_perception_msgs + autoware_perception_msgs cuda_utils cv_bridge eigen diff --git a/perception/bytetrack/src/bytetrack_node.cpp b/perception/bytetrack/src/bytetrack_node.cpp index eb5e73312a890..64adc83a51abd 100644 --- a/perception/bytetrack/src/bytetrack_node.cpp +++ b/perception/bytetrack/src/bytetrack_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" #include @@ -62,7 +62,7 @@ void ByteTrackNode::on_connect() void ByteTrackNode::on_rect( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr msg) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + using Label = autoware_perception_msgs::msg::ObjectClassification; tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects; tier4_perception_msgs::msg::DynamicObjectArray out_objects_uuid; @@ -97,7 +97,7 @@ void ByteTrackNode::on_rect( object.feature.roi.height = static_cast(output_height); object.object.existence_probability = tracked_object.score; object.object.classification.emplace_back( - autoware_auto_perception_msgs::build diff --git a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml b/planning/autoware_path_optimizer/launch/path_optimizer.launch.xml similarity index 65% rename from planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml rename to planning/autoware_path_optimizer/launch/path_optimizer.launch.xml index aec23bf3e60ad..273e3fecb377d 100644 --- a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml +++ b/planning/autoware_path_optimizer/launch/path_optimizer.launch.xml @@ -2,8 +2,8 @@ - - + + diff --git a/planning/obstacle_avoidance_planner/media/avoid_sudden_steering.drawio.svg b/planning/autoware_path_optimizer/media/avoid_sudden_steering.drawio.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/avoid_sudden_steering.drawio.svg rename to planning/autoware_path_optimizer/media/avoid_sudden_steering.drawio.svg diff --git a/planning/obstacle_avoidance_planner/media/avoidance_cost.drawio.svg b/planning/autoware_path_optimizer/media/avoidance_cost.drawio.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/avoidance_cost.drawio.svg rename to planning/autoware_path_optimizer/media/avoidance_cost.drawio.svg diff --git a/planning/obstacle_avoidance_planner/media/bounds.svg b/planning/autoware_path_optimizer/media/bounds.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/bounds.svg rename to planning/autoware_path_optimizer/media/bounds.svg diff --git a/planning/obstacle_avoidance_planner/media/debug/bounds_visualization.png b/planning/autoware_path_optimizer/media/debug/bounds_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/bounds_visualization.png rename to planning/autoware_path_optimizer/media/debug/bounds_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/calculation_time_plot.png b/planning/autoware_path_optimizer/media/debug/calculation_time_plot.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/calculation_time_plot.png rename to planning/autoware_path_optimizer/media/debug/calculation_time_plot.png diff --git a/planning/obstacle_avoidance_planner/media/debug/drivable_area_visualization.png b/planning/autoware_path_optimizer/media/debug/drivable_area_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/drivable_area_visualization.png rename to planning/autoware_path_optimizer/media/debug/drivable_area_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_fixed_traj_visualization.png b/planning/autoware_path_optimizer/media/debug/mpt_fixed_traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/mpt_fixed_traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/mpt_fixed_traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_ref_traj_visualization.png b/planning/autoware_path_optimizer/media/debug/mpt_ref_traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/mpt_ref_traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/mpt_ref_traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_traj_visualization.png b/planning/autoware_path_optimizer/media/debug/mpt_traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/mpt_traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/mpt_traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/path_footprint_visualization.png b/planning/autoware_path_optimizer/media/debug/path_footprint_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/path_footprint_visualization.png rename to planning/autoware_path_optimizer/media/debug/path_footprint_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/path_visualization.png b/planning/autoware_path_optimizer/media/debug/path_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/path_visualization.png rename to planning/autoware_path_optimizer/media/debug/path_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/traj_footprint_visualization.png b/planning/autoware_path_optimizer/media/debug/traj_footprint_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/traj_footprint_visualization.png rename to planning/autoware_path_optimizer/media/debug/traj_footprint_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/traj_visualization.png b/planning/autoware_path_optimizer/media/debug/traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/vehicle_circles_visualization.png b/planning/autoware_path_optimizer/media/debug/vehicle_circles_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/vehicle_circles_visualization.png rename to planning/autoware_path_optimizer/media/debug/vehicle_circles_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/vehicle_traj_circles_visualization.png b/planning/autoware_path_optimizer/media/debug/vehicle_traj_circles_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/vehicle_traj_circles_visualization.png rename to planning/autoware_path_optimizer/media/debug/vehicle_traj_circles_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/keep_minimum_boundary_width.drawio.svg b/planning/autoware_path_optimizer/media/keep_minimum_boundary_width.drawio.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/keep_minimum_boundary_width.drawio.svg rename to planning/autoware_path_optimizer/media/keep_minimum_boundary_width.drawio.svg diff --git a/planning/obstacle_avoidance_planner/media/mpt_optimization_offset.svg b/planning/autoware_path_optimizer/media/mpt_optimization_offset.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/mpt_optimization_offset.svg rename to planning/autoware_path_optimizer/media/mpt_optimization_offset.svg diff --git a/planning/obstacle_avoidance_planner/media/vehicle_circles.svg b/planning/autoware_path_optimizer/media/vehicle_circles.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/vehicle_circles.svg rename to planning/autoware_path_optimizer/media/vehicle_circles.svg diff --git a/planning/obstacle_avoidance_planner/media/vehicle_error_kinematics.png b/planning/autoware_path_optimizer/media/vehicle_error_kinematics.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/vehicle_error_kinematics.png rename to planning/autoware_path_optimizer/media/vehicle_error_kinematics.png diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/autoware_path_optimizer/package.xml similarity index 90% rename from planning/obstacle_avoidance_planner/package.xml rename to planning/autoware_path_optimizer/package.xml index 40caf7ef300aa..f02473ebd015e 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -1,9 +1,9 @@ - obstacle_avoidance_planner + autoware_path_optimizer 0.1.0 - The obstacle_avoidance_planner package + The autoware_path_optimizer package Takayuki Murooka Kosuke Takeuchi Takamasa Horibe @@ -14,7 +14,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz b/planning/autoware_path_optimizer/rviz/autoware_path_optimizer.rviz similarity index 98% rename from planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz rename to planning/autoware_path_optimizer/rviz/autoware_path_optimizer.rviz index c3cd89bbce9b9..4d4feb38f27a6 100644 --- a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz +++ b/planning/autoware_path_optimizer/rviz/autoware_path_optimizer.rviz @@ -761,7 +761,7 @@ Visualization Manager: CYCLIST: Alpha: 0.9990000128746033 Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Class: autoware_perception_rviz_plugin/DetectedObjects Display Acceleration: true Display Label: true Display PoseWithCovariance: true @@ -812,7 +812,7 @@ Visualization Manager: CYCLIST: Alpha: 0.9990000128746033 Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/TrackedObjects + Class: autoware_perception_rviz_plugin/TrackedObjects Display Acceleration: true Display Label: true Display PoseWithCovariance: true @@ -863,7 +863,7 @@ Visualization Manager: CYCLIST: Alpha: 0.9990000128746033 Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/PredictedObjects + Class: autoware_perception_rviz_plugin/PredictedObjects Display Acceleration: true Display Label: true Display PoseWithCovariance: false @@ -1038,7 +1038,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/eb_fixed_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/eb_fixed_traj Value: false View Footprint: Alpha: 1 @@ -1079,7 +1079,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/eb_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/eb_traj Value: false View Footprint: Alpha: 1 @@ -1124,7 +1124,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_ref_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/mpt_ref_traj Value: false View Footprint: Alpha: 1 @@ -1165,7 +1165,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_fixed_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/mpt_fixed_traj Value: false View Footprint: Alpha: 1 @@ -1206,7 +1206,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/marker Value: false - Class: rviz_plugins/Trajectory Color Border Vel Max: 3 @@ -1218,7 +1218,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/mpt_traj Value: false View Footprint: Alpha: 1 @@ -1263,7 +1263,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/trajectory Value: true View Footprint: Alpha: 1 diff --git a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py b/planning/autoware_path_optimizer/scripts/calculation_time_plotter.py similarity index 98% rename from planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py rename to planning/autoware_path_optimizer/scripts/calculation_time_plotter.py index 1904b45fb8225..29dfccb533fbe 100755 --- a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py +++ b/planning/autoware_path_optimizer/scripts/calculation_time_plotter.py @@ -33,7 +33,7 @@ def __init__(self, args): self.calculation_cost_hist = [] self.sub_calculation_cost = self.create_subscription( StringStamped, - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/calculation_time", + "/planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/calculation_time", self.CallbackCalculationCost, 1, ) diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp similarity index 98% rename from planning/obstacle_avoidance_planner/src/debug_marker.cpp rename to planning/autoware_path_optimizer/src/debug_marker.cpp index 2f8babf103877..3127d521160c2 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/debug_marker.hpp" +#include "autoware_path_optimizer/debug_marker.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -432,4 +432,4 @@ MarkerArray getDebugMarker( return marker_array; } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp similarity index 99% rename from planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp rename to planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 94dc62b1335d4..ece301e64c97e 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tf2/utils.h" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/normalization.hpp" @@ -29,7 +29,7 @@ #include #include -namespace obstacle_avoidance_planner +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 obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp similarity index 90% rename from planning/obstacle_avoidance_planner/src/node.cpp rename to planning/autoware_path_optimizer/src/node.cpp index c1da988eb5c25..49d41e6b07884 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/node.hpp" +#include "autoware_path_optimizer/node.hpp" +#include "autoware_path_optimizer/debug_marker.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "obstacle_avoidance_planner/debug_marker.hpp" -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace { @@ -82,8 +82,8 @@ std::vector calcSegmentLengthVector(const std::vector & } } // namespace -ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options) -: Node("obstacle_avoidance_planner", node_options), +PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) +: Node("path_optimizer", node_options), vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), debug_data_ptr_(std::make_shared()), time_keeper_ptr_(std::make_shared()) @@ -94,9 +94,7 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // interface subscriber path_sub_ = create_subscription( - "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); - odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); + "~/input/path", 1, std::bind(&PathOptimizer::onPath, this, std::placeholders::_1)); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -150,13 +148,13 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been // initialized. set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); + std::bind(&PathOptimizer::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); published_time_publisher_ = std::make_unique(this); } -rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( +rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( const std::vector & parameters) { using tier4_autoware_utils::updateParam; @@ -205,7 +203,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( return result; } -void ObstacleAvoidancePlanner::initializePlanning() +void PathOptimizer::initializePlanning() { RCLCPP_DEBUG(get_logger(), "Initialize planning"); @@ -214,18 +212,26 @@ void ObstacleAvoidancePlanner::initializePlanning() resetPreviousData(); } -void ObstacleAvoidancePlanner::resetPreviousData() +void PathOptimizer::resetPreviousData() { mpt_optimizer_ptr_->resetPreviousData(); } -void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) +void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); - // check if data is ready and valid - if (!isDataReady(*path_ptr, *get_clock())) { + // check if input path is valid + if (!checkInputPath(*path_ptr, *get_clock())) { + return; + } + + // check if ego's odometry is valid + const auto ego_odom_ptr = ego_odom_sub_.takeData(); + if (!ego_odom_ptr) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, "Waiting for ego pose and twist."); return; } @@ -245,7 +251,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) } // 1. create planner data - const auto planner_data = createPlannerData(*path_ptr); + const auto planner_data = createPlannerData(*path_ptr, ego_odom_ptr); // 2. generate optimized trajectory const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); @@ -276,13 +282,8 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); } -bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock clock) const +bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const { - if (!ego_state_ptr_) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist."); - return false; - } - if (path.points.size() < 2) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Path points size is less than 1."); return false; @@ -297,7 +298,8 @@ bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock cloc return true; } -PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const +PlannerData PathOptimizer::createPlannerData( + const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const { // create planner data PlannerData planner_data; @@ -305,14 +307,14 @@ PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points); planner_data.left_bound = path.left_bound; planner_data.right_bound = path.right_bound; - planner_data.ego_pose = ego_state_ptr_->pose.pose; - planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x; + planner_data.ego_pose = ego_odom_ptr->pose.pose; + planner_data.ego_vel = ego_odom_ptr->twist.twist.linear.x; debug_data_ptr_->ego_pose = planner_data.ego_pose; return planner_data; } -std::vector ObstacleAvoidancePlanner::generateOptimizedTrajectory( +std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); @@ -339,8 +341,7 @@ std::vector ObstacleAvoidancePlanner::generateOptimizedTrajecto return optimized_traj_points; } -std::vector ObstacleAvoidancePlanner::optimizeTrajectory( - const PlannerData & planner_data) +std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); const auto & p = planner_data; @@ -373,7 +374,7 @@ std::vector ObstacleAvoidancePlanner::optimizeTrajectory( return mpt_traj; } -std::vector ObstacleAvoidancePlanner::getPrevOptimizedTrajectory( +std::vector PathOptimizer::getPrevOptimizedTrajectory( const std::vector & traj_points) const { const auto prev_optimized_traj_points = mpt_optimizer_ptr_->getPrevOptimizedTrajectoryPoints(); @@ -383,7 +384,7 @@ std::vector ObstacleAvoidancePlanner::getPrevOptimizedTrajector return traj_points; } -void ObstacleAvoidancePlanner::applyInputVelocity( +void PathOptimizer::applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const @@ -486,7 +487,7 @@ void ObstacleAvoidancePlanner::applyInputVelocity( time_keeper_ptr_->toc(__func__, " "); } -void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( +void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { time_keeper_ptr_->tic(__func__); @@ -553,7 +554,7 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( time_keeper_ptr_->toc(__func__, " "); } -void ObstacleAvoidancePlanner::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const +void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { time_keeper_ptr_->tic(__func__); @@ -568,7 +569,7 @@ void ObstacleAvoidancePlanner::publishVirtualWall(const geometry_msgs::msg::Pose time_keeper_ptr_->toc(__func__, " "); } -void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization( +void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { if (!enable_pub_debug_marker_) { @@ -590,7 +591,7 @@ void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization( time_keeper_ptr_->toc(__func__, " "); } -std::vector ObstacleAvoidancePlanner::extendTrajectory( +std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { @@ -654,7 +655,7 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( return resampled_traj_points; } -void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const +void PathOptimizer::publishDebugData(const Header & header) const { time_keeper_ptr_->tic(__func__); @@ -665,7 +666,7 @@ void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const time_keeper_ptr_->toc(__func__, " "); } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_avoidance_planner::ObstacleAvoidancePlanner) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware_path_optimizer::PathOptimizer) diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp similarity index 97% rename from planning/obstacle_avoidance_planner/src/replan_checker.cpp rename to planning/autoware_path_optimizer/src/replan_checker.cpp index 5f495051cc2bd..797041ee75416 100644 --- a/planning/obstacle_avoidance_planner/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/replan_checker.hpp" +#include "autoware_path_optimizer/replan_checker.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include -namespace obstacle_avoidance_planner +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 obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp similarity index 92% rename from planning/obstacle_avoidance_planner/src/state_equation_generator.cpp rename to planning/autoware_path_optimizer/src/state_equation_generator.cpp index 7460ce9c1f764..7712fbbf6c3cf 100644 --- a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/state_equation_generator.hpp" +#include "autoware_path_optimizer/state_equation_generator.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" -namespace obstacle_avoidance_planner +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 obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp similarity index 95% rename from planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp rename to planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 3f35de9147e6a..9d93cdc26a7ed 100644 --- a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "tf2/utils.h" #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" @@ -36,7 +36,7 @@ #include #include -namespace obstacle_avoidance_planner +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 obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp similarity index 92% rename from planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp rename to planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 651b11b28e8bc..7983c5c2a3c2f 100644 --- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "tf2/utils.h" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" @@ -36,25 +36,25 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p) +geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p) { return p.pose.position; } template <> -geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p) +geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p) { return p.pose; } template <> -double getLongitudinalVelocity(const obstacle_avoidance_planner::ReferencePoint & p) +double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p) { return p.longitudinal_velocity_mps; } } // namespace tier4_autoware_utils -namespace obstacle_avoidance_planner +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 obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp similarity index 94% rename from planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp rename to planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 19ff1775a1211..89e9e0e2c9b74 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/planning/autoware_path_optimizer/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 "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp similarity index 92% rename from planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp rename to planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp index 07a2ab00e7546..57e4671a4e0cc 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp +++ b/planning/autoware_path_optimizer/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 "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp" VehicleModelInterface::VehicleModelInterface( const int dim_x, const int dim_u, const int dim_y, const double wheelbase, diff --git a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp similarity index 70% rename from planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp rename to planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index d5af4c7e1180f..8ef099ba09f24 100644 --- a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/node.hpp" +#include "autoware_path_optimizer/node.hpp" #include #include @@ -32,27 +32,26 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto obstacle_avoidance_planner_dir = - ament_index_cpp::get_package_share_directory("obstacle_avoidance_planner"); + const auto path_optimizer_dir = + ament_index_cpp::get_package_share_directory("autoware_path_optimizer"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_common.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", - obstacle_avoidance_planner_dir + "/config/obstacle_avoidance_planner.param.yaml"}); + 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, "obstacle_avoidance_planner/input/odometry"); + test_manager->publishOdometry(test_target_node, "path_optimizer/input/odometry"); - // set subscriber with topic name: obstacle_avoidance_planner → test_node_ - test_manager->setTrajectorySubscriber("obstacle_avoidance_planner/output/path"); + // set subscriber with topic name: path_optimizer → test_node_ + test_manager->setTrajectorySubscriber("path_optimizer/output/path"); - // set obstacle_avoidance_planner's input topic name(this topic is changed to test node) - test_manager->setPathInputTopicName("obstacle_avoidance_planner/input/path"); + // set path_optimizer's input topic name(this topic is changed to test node) + test_manager->setPathInputTopicName("path_optimizer/input/path"); // test with normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node)); diff --git a/planning/autoware_planning_test_manager/README.md b/planning/autoware_planning_test_manager/README.md index 63f1bf53a4954..fe5484ca498ee 100644 --- a/planning/autoware_planning_test_manager/README.md +++ b/planning/autoware_planning_test_manager/README.md @@ -1,4 +1,4 @@ -# Planning Interface Test Manager +# Autoware Planning Test Manager ## Background @@ -49,7 +49,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) // publish the necessary topics from test_manager second argument is topic name test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); test_manager->publishMaxVelocity( - test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); + test_target_node, "velocity_smoother/input/external_velocity_limit_mps"); // set scenario_selector's input topic name(this topic is changed to test node) test_manager->setTrajectoryInputTopicName("input/parking/trajectory"); @@ -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 | -| motion_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 | -| obstacle_avoidance_planner | 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 | +| ------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | +| 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/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp index 82636af20579d..08a6e1b9d6960 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp @@ -35,15 +35,14 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include +#include +#include +#include #include #include #include @@ -57,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -72,13 +72,12 @@ namespace planning_test_utils { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseStamped; @@ -92,6 +91,7 @@ using tier4_api_msgs::msg::CrosswalkStatus; using tier4_api_msgs::msg::IntersectionStatus; using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::VelocityLimit; using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; @@ -190,7 +190,7 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr expand_stop_range_pub_; rclcpp::Publisher::SharedPtr occupancy_grid_pub_; rclcpp::Publisher::SharedPtr cost_map_pub_; - rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::Publisher::SharedPtr map_pub_; rclcpp::Publisher::SharedPtr scenario_pub_; rclcpp::Publisher::SharedPtr parking_scenario_pub_; rclcpp::Publisher::SharedPtr lane_driving_scenario_pub_; @@ -201,7 +201,7 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr initial_pose_tf_pub_; rclcpp::Publisher::SharedPtr lateral_offset_pub_; rclcpp::Publisher::SharedPtr operation_mode_state_pub_; - rclcpp::Publisher::SharedPtr traffic_signals_pub_; + rclcpp::Publisher::SharedPtr traffic_signals_pub_; rclcpp::Publisher::SharedPtr virtual_traffic_light_states_pub_; // Subscriber diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index e2c00756c2ba4..3170ff5f854b9 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -13,12 +13,11 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils lanelet2_extension diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 9ee162ec02d1f..01a8d89ca8b4d 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -171,7 +171,7 @@ void PlanningInterfaceTestManager::publishTrafficSignals( rclcpp::Node::SharedPtr target_node, std::string topic_name) { test_utils::publishToTargetNode( - test_node_, target_node, topic_name, traffic_signals_pub_, TrafficSignalArray{}); + test_node_, target_node, topic_name, traffic_signals_pub_, TrafficLightGroupArray{}); } void PlanningInterfaceTestManager::publishVirtualTrafficLightState( @@ -293,7 +293,7 @@ void PlanningInterfaceTestManager::publishNominalPath( { test_utils::publishToTargetNode( test_node_, target_node, topic_name, normal_path_pub_, - motion_utils::convertToPath( + motion_utils::convertToPath( test_utils::loadPathWithLaneIdInYaml()), 5); } diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp index 2a88cdb57abf4..c87c397bd07aa 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include #include @@ -50,7 +50,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node private: using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using Odometry = nav_msgs::msg::Odometry; using VelocityLimit = tier4_planning_msgs::msg::VelocityLimit; using MissionRemainingDistanceTime = autoware_internal_msgs::msg::MissionRemainingDistanceTime; diff --git a/planning/autoware_static_centerline_generator/README.md b/planning/autoware_static_centerline_generator/README.md index 844d0af15f2a5..426d5487cf0cb 100644 --- a/planning/autoware_static_centerline_generator/README.md +++ b/planning/autoware_static_centerline_generator/README.md @@ -5,7 +5,7 @@ This package statically calculates the centerline satisfying path footprints inside the drivable area. On narrow-road driving, the default centerline, which is the middle line between lanelets' right and left boundaries, often causes path footprints outside the drivable area. -To make path footprints inside the drivable area, we use online path shape optimization by [the obstacle_avoidance_planner package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/obstacle_avoidance_planner/). +To make path footprints inside the drivable area, we use online path shape optimization by [the autoware_path_optimizer package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/autoware_path_optimizer/). Instead of online path shape optimization, we introduce static centerline optimization. With this static centerline optimization, we have following advantages. diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml index 8e016e2b63391..486ea547e3dc1 100644 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -28,10 +28,7 @@ default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml" /> - + @@ -76,7 +73,7 @@ - + diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 96e17595d49ee..6d6bfb794940a 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -15,9 +15,10 @@ rosidl_default_generators - autoware_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_map_msgs + autoware_path_optimizer + autoware_perception_msgs + autoware_planning_msgs behavior_path_planner_common geography_utils geometry_msgs @@ -28,7 +29,6 @@ map_projection_loader mission_planner motion_utils - obstacle_avoidance_planner osqp_interface path_smoother rclcpp @@ -43,9 +43,9 @@ ament_cmake_gmock ament_lint_auto + autoware_behavior_velocity_planner autoware_lint_common behavior_path_planner - behavior_velocity_planner ros_testing rosidl_interface_packages diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py index fec3d21d20bec..3672165caed85 100755 --- a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py +++ b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py @@ -26,7 +26,7 @@ from PyQt5.QtWidgets import QSizePolicy from PyQt5.QtWidgets import QSlider from PyQt5.QtWidgets import QWidget -from autoware_auto_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import Trajectory import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy 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 2825381937674..9eb9a2b432a21 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 @@ -14,9 +14,9 @@ #include "centerline_source/optimization_trajectory_based_centerline.hpp" +#include "autoware_path_optimizer/node.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "obstacle_avoidance_planner/node.hpp" #include "path_smoother/elastic_band_smoother.hpp" #include "static_centerline_generator_node.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" @@ -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 = - obstacle_avoidance_planner::ObstacleAvoidancePlanner(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; @@ -156,9 +156,9 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra const auto smoothed_traj_points = eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); - // road collision avoidance by model predictive trajectory in the obstacle_avoidance_planner + // road collision avoidance by model predictive trajectory in the autoware_path_optimizer // package - const obstacle_avoidance_planner::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/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 74002916bb23c..24e95353eec93 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -173,7 +173,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( { // publishers pub_map_bin_ = - create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); + create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); pub_whole_centerline_ = create_publisher("output_whole_centerline", utils::create_transient_local_qos()); pub_centerline_ = @@ -357,7 +357,7 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ std::filesystem::copy_options::overwrite_existing); // load map by the map_loader package - map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { + map_bin_ptr_ = [&]() -> LaneletMapBin::ConstSharedPtr { // load map map_projector_info_ = std::make_unique(load_info_from_lanelet2_map(lanelet2_input_file_path)); @@ -379,7 +379,7 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ const auto map_bin_msg = Lanelet2MapLoaderNode::create_map_bin_msg(map_ptr, lanelet2_input_file_path, now()); - return std::make_shared(map_bin_msg); + return std::make_shared(map_bin_msg); }(); // check if map_bin_ptr_ is not null pointer diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp index dfe5af68c270b..f8a65f2794809 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp @@ -80,7 +80,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node const CenterlineWithRoute & centerline_with_route); lanelet::LaneletMapPtr original_map_ptr_{nullptr}; - HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; std::shared_ptr route_handler_ptr_{nullptr}; std::unique_ptr map_projector_info_{nullptr}; @@ -95,7 +95,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_; // publisher - rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; + rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr}; rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp index fb54804db105d..d40e519ef2704 100644 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp @@ -17,29 +17,29 @@ #include "route_handler/route_handler.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/lanelet_route.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace autoware::static_centerline_generator { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; +using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index 3011abccd48ca..7a5cdfd905fe9 100644 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -59,7 +59,7 @@ def generate_test_description(): "config/behavior_path_planner.param.yaml", ), os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("autoware_behavior_velocity_planner"), "config/behavior_velocity_planner.param.yaml", ), os.path.join( @@ -67,8 +67,8 @@ def generate_test_description(): "config/elastic_band_smoother.param.yaml", ), os.path.join( - get_package_share_directory("obstacle_avoidance_planner"), - "config/obstacle_avoidance_planner.param.yaml", + get_package_share_directory("autoware_path_optimizer"), + "config/path_optimizer.param.yaml", ), os.path.join( get_package_share_directory("map_loader"), diff --git a/planning/motion_velocity_smoother/CMakeLists.txt b/planning/autoware_velocity_smoother/CMakeLists.txt similarity index 72% rename from planning/motion_velocity_smoother/CMakeLists.txt rename to planning/autoware_velocity_smoother/CMakeLists.txt index 7fd9c5681315c..fee13513a7a97 100644 --- a/planning/motion_velocity_smoother/CMakeLists.txt +++ b/planning/autoware_velocity_smoother/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(motion_velocity_smoother) +project(autoware_velocity_smoother) find_package(autoware_cmake REQUIRED) autoware_package() @@ -14,7 +14,7 @@ include_directories( ) set(MOTION_VELOCITY_SMOOTHER_SRC - src/motion_velocity_smoother_node.cpp + src/node.cpp ) set(SMOOTHER_SRC @@ -32,17 +32,17 @@ ament_auto_add_library(smoother SHARED ${SMOOTHER_SRC} ) -ament_auto_add_library(motion_velocity_smoother_node SHARED +ament_auto_add_library(${PROJECT_NAME}_node SHARED ${MOTION_VELOCITY_SMOOTHER_SRC} ) -target_link_libraries(motion_velocity_smoother_node +target_link_libraries(${PROJECT_NAME}_node smoother ) -rclcpp_components_register_node(motion_velocity_smoother_node - PLUGIN "motion_velocity_smoother::MotionVelocitySmootherNode" - EXECUTABLE motion_velocity_smoother +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "autoware_velocity_smoother::VelocitySmootherNode" + EXECUTABLE velocity_smoother_node ) if(BUILD_TESTING) @@ -53,10 +53,10 @@ if(BUILD_TESTING) smoother ) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_motion_velocity_smoother_node_interface.cpp + test/test_velocity_smoother_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} - motion_velocity_smoother_node + ${PROJECT_NAME}_node ) endif() diff --git a/planning/motion_velocity_smoother/README.ja.md b/planning/autoware_velocity_smoother/README.ja.md similarity index 88% rename from planning/motion_velocity_smoother/README.ja.md rename to planning/autoware_velocity_smoother/README.ja.md index 62e97b6bbc906..d36a328ddf7ca 100644 --- a/planning/motion_velocity_smoother/README.ja.md +++ b/planning/autoware_velocity_smoother/README.ja.md @@ -1,10 +1,10 @@ -# Motion Velocity Smoother +# Velocity Smoother ## Purpose -`motion_velocity_smoother`は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 +`autoware_velocity_smoother`は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 -加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールを`motion_velocity_smoother`と呼んでいる。 +加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールを`autoware_velocity_smoother`と呼んでいる。 ## Inner-workings / Algorithms @@ -80,29 +80,29 @@ ### Input -| Name | Type | Description | -| ------------------------------------------ | ---------------------------------------- | ----------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | -| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | -| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | -| `/tf` | `tf2_msgs/TFMessage` | TF | -| `/tf_static` | `tf2_msgs/TFMessage` | TF static | +| Name | Type | Description | +| ------------------------------------------ | ----------------------------------- | ----------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | +| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | +| `/tf` | `tf2_msgs/TFMessage` | TF | +| `/tf_static` | `tf2_msgs/TFMessage` | TF static | ### Output -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------------------- | -| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory | -| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | -| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | -| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | -| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | -| `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) | -| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | -| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | -| `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | -| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | -| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------- | --------------------------------------------------------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Modified trajectory | +| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | +| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | +| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | +| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | +| `~/debug/trajectory_raw` | `autoware_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | +| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | +| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | ## Parameters diff --git a/planning/motion_velocity_smoother/README.md b/planning/autoware_velocity_smoother/README.md similarity index 88% rename from planning/motion_velocity_smoother/README.md rename to planning/autoware_velocity_smoother/README.md index 1ff7f5982b4eb..1a506d8612aa6 100644 --- a/planning/motion_velocity_smoother/README.md +++ b/planning/autoware_velocity_smoother/README.md @@ -1,10 +1,10 @@ -# Motion Velocity Smoother +# Velocity Smoother ## Purpose -`motion_velocity_smoother` outputs a desired velocity profile on a reference trajectory. +`autoware_velocity_smoother` outputs a desired velocity profile on a reference trajectory. This module plans a velocity profile within the limitations of the velocity, the acceleration and the jerk to realize both the maximization of velocity and the ride quality. -We call this module `motion_velocity_smoother` because the limitations of the acceleration and the jerk means the smoothness of the velocity profile. +We call this module `autoware_velocity_smoother` because the limitations of the acceleration and the jerk means the smoothness of the velocity profile. ## Inner-workings / Algorithms @@ -18,7 +18,7 @@ For the point on the reference trajectory closest to the center of the rear whee #### Apply external velocity limit -It applies the velocity limit input from the external of `motion_velocity_smoother`. +It applies the velocity limit input from the external of `autoware_velocity_smoother`. Remark that the external velocity limit is different from the velocity limit already set on the map and the reference trajectory. The external velocity is applied at the position that it is able to reach the velocity limit with the deceleration and the jerk constraints set as the parameter. @@ -92,30 +92,30 @@ After the optimization, a resampling called `post resampling` is performed befor ### Input -| Name | Type | Description | -| ------------------------------------------ | ---------------------------------------- | ----------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | -| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | -| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | -| `/tf` | `tf2_msgs/TFMessage` | TF | -| `/tf_static` | `tf2_msgs/TFMessage` | TF static | +| Name | Type | Description | +| ------------------------------------------ | ----------------------------------- | ----------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | +| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | +| `/tf` | `tf2_msgs/TFMessage` | TF | +| `/tf_static` | `tf2_msgs/TFMessage` | TF static | ### Output -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------------------- | -| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory | -| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | -| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | -| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | -| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | -| `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) | -| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | -| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | -| `~/debug/trajectory_steering_rate_limited` | `autoware_auto_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) | -| `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | -| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | -| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------- | --------------------------------------------------------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Modified trajectory | +| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | +| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | +| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | +| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | +| `~/debug/trajectory_raw` | `autoware_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_steering_rate_limited` | `autoware_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | +| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | +| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | ## Parameters diff --git a/planning/motion_velocity_smoother/config/Analytical.param.yaml b/planning/autoware_velocity_smoother/config/Analytical.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/Analytical.param.yaml rename to planning/autoware_velocity_smoother/config/Analytical.param.yaml diff --git a/planning/motion_velocity_smoother/config/JerkFiltered.param.yaml b/planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/JerkFiltered.param.yaml rename to planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml diff --git a/planning/motion_velocity_smoother/config/L2.param.yaml b/planning/autoware_velocity_smoother/config/L2.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/L2.param.yaml rename to planning/autoware_velocity_smoother/config/L2.param.yaml diff --git a/planning/motion_velocity_smoother/config/Linf.param.yaml b/planning/autoware_velocity_smoother/config/Linf.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/Linf.param.yaml rename to planning/autoware_velocity_smoother/config/Linf.param.yaml diff --git a/planning/motion_velocity_smoother/config/default_common.param.yaml b/planning/autoware_velocity_smoother/config/default_common.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/default_common.param.yaml rename to planning/autoware_velocity_smoother/config/default_common.param.yaml diff --git a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml b/planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml rename to planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml diff --git a/planning/motion_velocity_smoother/config/rqt_multiplot.xml b/planning/autoware_velocity_smoother/config/rqt_multiplot.xml similarity index 96% rename from planning/motion_velocity_smoother/config/rqt_multiplot.xml rename to planning/autoware_velocity_smoother/config/rqt_multiplot.xml index ad0ff4b68251e..694f18c0d9b4f 100644 --- a/planning/motion_velocity_smoother/config/rqt_multiplot.xml +++ b/planning/autoware_velocity_smoother/config/rqt_multiplot.xml @@ -35,7 +35,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_velocity + /planning/scenario_planning/velocity_smoother/closest_velocity std_msgs/Float32 @@ -48,7 +48,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_velocity + /planning/scenario_planning/velocity_smoother/closest_velocity std_msgs/Float32 @@ -86,7 +86,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_merged_velocity + /planning/scenario_planning/velocity_smoother/closest_merged_velocity std_msgs/Float32 @@ -99,7 +99,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_merged_velocity + /planning/scenario_planning/velocity_smoother/closest_merged_velocity std_msgs/Float32 @@ -137,7 +137,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_max_velocity + /planning/scenario_planning/velocity_smoother/closest_max_velocity std_msgs/Float32 @@ -150,7 +150,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_max_velocity + /planning/scenario_planning/velocity_smoother/closest_max_velocity std_msgs/Float32 @@ -188,7 +188,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_acceleration + /planning/scenario_planning/velocity_smoother/closest_acceleration std_msgs/Float32 @@ -201,7 +201,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_acceleration + /planning/scenario_planning/velocity_smoother/closest_acceleration std_msgs/Float32 @@ -264,7 +264,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/calculation_time + /planning/scenario_planning/velocity_smoother/calculation_time std_msgs/Float32 @@ -277,7 +277,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/calculation_time + /planning/scenario_planning/velocity_smoother/calculation_time std_msgs/Float32 diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp similarity index 90% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp index 868c1ab12cce8..aecfab342e7e0 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ -#define MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ - +#ifndef AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ + +#include "autoware_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/resample.hpp" -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" -#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -37,8 +37,8 @@ #include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" // temporary @@ -53,10 +53,10 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using autoware_adapi_v1_msgs::msg::OperationModeState; using geometry_msgs::msg::AccelWithCovarianceStamped; @@ -77,10 +77,10 @@ struct Motion Motion(const double v, const double a) : vel(v), acc(a) {} }; -class MotionVelocitySmootherNode : public rclcpp::Node +class VelocitySmootherNode : public rclcpp::Node { public: - explicit MotionVelocitySmootherNode(const rclcpp::NodeOptions & node_options); + explicit VelocitySmootherNode(const rclcpp::NodeOptions & node_options); private: rclcpp::Publisher::SharedPtr pub_trajectory_; @@ -275,6 +275,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp similarity index 85% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp index 80f691b5f7b40..dc85c2b2f336f 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ -#define MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace resampling { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; struct ResampleParam @@ -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 motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_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 similarity index 84% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 9cddba5ca500a..e54a6e7b8afad 100644 --- a/planning/motion_velocity_smoother/include/motion_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 @@ -12,23 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { class AnalyticalJerkConstrainedSmoother : public SmootherBase { @@ -113,8 +113,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase std::string strTimes(const std::vector & times) const; std::string strStartIndices(const std::vector & start_indices) const; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother // clang-format off -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT // clang-format on diff --git a/planning/motion_velocity_smoother/include/motion_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 similarity index 74% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 554bdcca890ef..3ab4fce11fd0b 100644 --- a/planning/motion_velocity_smoother/include/motion_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 @@ -12,25 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace analytical_velocity_planning_utils { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; bool calcStopDistWithJerkAndAccConstraints( const double v0, const double a0, const double jerk_acc, const double jerk_dec, @@ -51,8 +51,8 @@ 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 motion_velocity_smoother +} // namespace autoware_velocity_smoother // clang-format off -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT // clang-format on diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp similarity index 84% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 4b55fbbf184b9..09c1e7e96be7b 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "boost/optional.hpp" #include -namespace motion_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 motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp similarity index 79% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 392fef2429736..066e0acef67f6 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "boost/optional.hpp" #include -namespace motion_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 motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp similarity index 79% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 9cc6ffef16090..b54de318e9702 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "boost/optional.hpp" #include -namespace motion_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 motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp similarity index 88% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp index beb571635f70c..43ccfcf193f14 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#include "motion_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/resample.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; class SmootherBase @@ -86,6 +86,6 @@ class SmootherBase protected: BaseParam base_param_; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp similarity index 85% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp index 35e8ab53dd75d..d35f80fb7fad8 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" -#include "geometry_msgs/msg/detail/pose__struct.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" #include #include #include #include -namespace motion_velocity_smoother::trajectory_utils +namespace autoware_velocity_smoother::trajectory_utils { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Pose; @@ -77,6 +77,6 @@ std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest); -} // namespace motion_velocity_smoother::trajectory_utils +} // namespace autoware_velocity_smoother::trajectory_utils -#endif // MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml b/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml similarity index 77% rename from planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml rename to planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml index 86f767560fd7c..11932ad125adc 100644 --- a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml +++ b/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml @@ -1,6 +1,6 @@ - + @@ -12,10 +12,10 @@ - - + + - + diff --git a/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg b/planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg similarity index 100% rename from planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg rename to planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg diff --git a/planning/motion_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml similarity index 91% rename from planning/motion_velocity_smoother/package.xml rename to planning/autoware_velocity_smoother/package.xml index b9b368d917535..4b5a46e495545 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -1,9 +1,9 @@ - motion_velocity_smoother + autoware_velocity_smoother 0.1.0 - The motion_velocity_smoother package + The autoware_velocity_smoother package Fumiya Watanabe Takamasa Horibe @@ -20,7 +20,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/autoware_velocity_smoother/src/node.cpp similarity index 91% rename from planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp rename to planning/autoware_velocity_smoother/src/node.cpp index eb8592bb99637..0ea10534eb126 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/motion_velocity_smoother_node.hpp" +#include "autoware_velocity_smoother/node.hpp" +#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "motion_utils/marker/marker_helper.hpp" -#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -32,10 +32,10 @@ #include // clang-format on -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { -MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions & node_options) -: Node("motion_velocity_smoother", node_options) +VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) +: Node("velocity_smoother", node_options) { using std::placeholders::_1; @@ -57,13 +57,13 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); pub_over_stop_velocity_ = create_publisher("~/stop_speed_exceeded", 1); sub_current_trajectory_ = create_subscription( - "~/input/trajectory", 1, std::bind(&MotionVelocitySmootherNode::onCurrentTrajectory, this, _1)); + "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); sub_current_odometry_ = create_subscription( "/localization/kinematic_state", 1, - std::bind(&MotionVelocitySmootherNode::onCurrentOdometry, this, _1)); + std::bind(&VelocitySmootherNode::onCurrentOdometry, this, _1)); sub_external_velocity_limit_ = create_subscription( "~/input/external_velocity_limit_mps", 1, - std::bind(&MotionVelocitySmootherNode::onExternalVelocityLimit, this, _1)); + std::bind(&VelocitySmootherNode::onExternalVelocityLimit, this, _1)); sub_current_acceleration_ = create_subscription( "~/input/acceleration", 1, [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { current_acceleration_ptr_ = msg; @@ -73,8 +73,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; }); // parameter update - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&MotionVelocitySmootherNode::onParameter, this, _1)); + set_param_res_ = + this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1)); // debug publish_debug_trajs_ = declare_parameter("publish_debug_trajs"); @@ -107,7 +107,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions published_time_publisher_ = std::make_unique(this); } -void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) +void VelocitySmootherNode::setupSmoother(const double wheelbase) { switch (node_param_.algorithm_type) { case AlgorithmType::JERK_FILTERED: { @@ -137,13 +137,13 @@ void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) break; } default: - throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); + throw std::domain_error("[VelocitySmootherNode] invalid algorithm"); } smoother_->setWheelBase(wheelbase); } -rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter( +rcl_interfaces::msg::SetParametersResult VelocitySmootherNode::onParameter( const std::vector & parameters) { auto update_param = [&](const std::string & name, double & v) { @@ -263,7 +263,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter break; } default: - throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); + throw std::domain_error("[VelocitySmootherNode] invalid algorithm"); } rcl_interfaces::msg::SetParametersResult result{}; @@ -272,7 +272,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter return result; } -void MotionVelocitySmootherNode::initCommonParam() +void VelocitySmootherNode::initCommonParam() { auto & p = node_param_; p.enable_lateral_acc_limit = declare_parameter("enable_lateral_acc_limit"); @@ -310,7 +310,7 @@ void MotionVelocitySmootherNode::initCommonParam() declare_parameter("plan_from_ego_speed_on_manual_mode"); } -void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const +void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const { Trajectory publishing_trajectory = motion_utils::convertToTrajectory(trajectory); publishing_trajectory.header = base_traj_raw_ptr_->header; @@ -319,17 +319,17 @@ void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & traj pub_trajectory_, publishing_trajectory.header.stamp); } -void MotionVelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) +void VelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) { current_odometry_ptr_ = msg; } -void MotionVelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) +void VelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) { external_velocity_limit_ptr_ = msg; } -void MotionVelocitySmootherNode::calcExternalVelocityLimit() +void VelocitySmootherNode::calcExternalVelocityLimit() { if (!external_velocity_limit_ptr_) { return; @@ -418,7 +418,7 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit() return; } -bool MotionVelocitySmootherNode::checkData() const +bool VelocitySmootherNode::checkData() const { if (!current_odometry_ptr_ || !base_traj_raw_ptr_ || !current_acceleration_ptr_) { RCLCPP_DEBUG( @@ -434,7 +434,7 @@ bool MotionVelocitySmootherNode::checkData() const return true; } -void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) +void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) { RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); @@ -516,7 +516,7 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n"); } -void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit() +void VelocitySmootherNode::updateDataForExternalVelocityLimit() { if (prev_output_.empty()) { return; @@ -532,7 +532,7 @@ void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit() return; } -TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( +TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity( const TrajectoryPoints & traj_input) const { TrajectoryPoints output{}; // velocity is optimized by qp solver @@ -578,7 +578,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( return output; } -bool MotionVelocitySmootherNode::smoothVelocity( +bool VelocitySmootherNode::smoothVelocity( const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed) const { @@ -685,7 +685,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( return true; } -void MotionVelocitySmootherNode::insertBehindVelocity( +void VelocitySmootherNode::insertBehindVelocity( const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const { const bool keep_closest_vel_for_behind = @@ -727,7 +727,7 @@ void MotionVelocitySmootherNode::insertBehindVelocity( } } -void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const +void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const { const size_t closest = findNearestIndexFromEgo(trajectory); @@ -746,8 +746,7 @@ void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & tr pub_dist_to_stopline_->publish(dist_to_stopline); } -std::pair -MotionVelocitySmootherNode::calcInitialMotion( +std::pair VelocitySmootherNode::calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest) const { const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); @@ -830,7 +829,7 @@ MotionVelocitySmootherNode::calcInitialMotion( return {initial_motion, InitializeType::NORMAL}; } -void MotionVelocitySmootherNode::overwriteStopPoint( +void VelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { const auto stop_idx = motion_utils::searchZeroVelocityIndex(input); @@ -877,7 +876,7 @@ void MotionVelocitySmootherNode::overwriteStopPoint( } } -void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const +void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const { if (traj.size() < 1) { return; @@ -915,7 +914,7 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_.velocity); } -void MotionVelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const +void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { @@ -933,7 +932,7 @@ void MotionVelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & } } -void MotionVelocitySmootherNode::publishDebugTrajectories( +void VelocitySmootherNode::publishDebugTrajectories( const std::vector & debug_trajectories) const { auto debug_trajectories_tmp = debug_trajectories; @@ -955,7 +954,7 @@ void MotionVelocitySmootherNode::publishDebugTrajectories( } } -void MotionVelocitySmootherNode::publishClosestVelocity( +void VelocitySmootherNode::publishClosestVelocity( const TrajectoryPoints & trajectory, const Pose & current_pose, const rclcpp::Publisher::SharedPtr pub) const { @@ -967,7 +966,7 @@ void MotionVelocitySmootherNode::publishClosestVelocity( pub->publish(vel_data); } -void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) +void VelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) { const auto closest_point = calcProjectedTrajectoryPointFromEgo(trajectory); @@ -1002,13 +1001,13 @@ void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & tr *prev_time_ = curr_time; } -void MotionVelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result) +void VelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result) { prev_output_ = final_result; prev_closest_point_ = calcProjectedTrajectoryPointFromEgo(final_result); } -MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorithmType( +VelocitySmootherNode::AlgorithmType VelocitySmootherNode::getAlgorithmType( const std::string & algorithm_name) const { if (algorithm_name == "JerkFiltered") { @@ -1024,11 +1023,11 @@ MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorit return AlgorithmType::ANALYTICAL; } - throw std::domain_error("[MotionVelocitySmootherNode] undesired algorithm is selected."); + throw std::domain_error("[VelocitySmootherNode] undesired algorithm is selected."); return AlgorithmType::INVALID; } -double MotionVelocitySmootherNode::calcTravelDistance() const +double VelocitySmootherNode::calcTravelDistance() const { const auto closest_point = calcProjectedTrajectoryPointFromEgo(prev_output_); @@ -1041,14 +1040,14 @@ double MotionVelocitySmootherNode::calcTravelDistance() const return 0.0; } -bool MotionVelocitySmootherNode::isEngageStatus(const double target_vel) const +bool VelocitySmootherNode::isEngageStatus(const double target_vel) const { const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio; return vehicle_speed < engage_vel_thr && target_vel >= node_param_.engage_velocity; } -Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( +Trajectory VelocitySmootherNode::toTrajectoryMsg( const TrajectoryPoints & points, const std_msgs::msg::Header * header) const { auto trajectory = motion_utils::convertToTrajectory(points); @@ -1056,28 +1055,28 @@ Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( return trajectory; } -size_t MotionVelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const +size_t VelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const { return motion_utils::findFirstNearestIndexWithSoftConstraints( points, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); } -bool MotionVelocitySmootherNode::isReverse(const TrajectoryPoints & points) const +bool VelocitySmootherNode::isReverse(const TrajectoryPoints & points) const { if (points.empty()) return true; return std::any_of( points.begin(), points.end(), [](auto & pt) { return pt.longitudinal_velocity_mps < 0; }); } -void MotionVelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const +void VelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const { for (auto & pt : points) { pt.longitudinal_velocity_mps *= -1.0; } } -void MotionVelocitySmootherNode::publishStopWatchTime() +void VelocitySmootherNode::publishStopWatchTime() { Float32Stamped calculation_time_data{}; calculation_time_data.stamp = this->now(); @@ -1085,7 +1084,7 @@ void MotionVelocitySmootherNode::publishStopWatchTime() debug_calculation_time_->publish(calculation_time_data); } -TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPoint( +TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPoint( const TrajectoryPoints & trajectory, const Pose & pose) const { const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -1094,13 +1093,13 @@ TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPoint( return trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, pose, current_seg_idx); } -TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( +TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( const TrajectoryPoints & trajectory) const { return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(motion_velocity_smoother::MotionVelocitySmootherNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware_velocity_smoother::VelocitySmootherNode) diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp similarity index 98% rename from planning/motion_velocity_smoother/src/resample.cpp rename to planning/autoware_velocity_smoother/src/resample.cpp index ae638f8e4db1a..17ea1eb9fc9bd 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace resampling { @@ -264,4 +264,4 @@ TrajectoryPoints resampleTrajectory( } } // namespace resampling -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_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 similarity index 98% rename from planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 84d6331b3429d..9cdfc515d51b7 100644 --- a/planning/motion_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 @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include #include @@ -26,7 +26,7 @@ namespace { -using TrajectoryPoints = std::vector; +using TrajectoryPoints = std::vector; geometry_msgs::msg::Pose lerpByPose( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) @@ -64,7 +64,7 @@ bool applyMaxVelocity( } // namespace -namespace motion_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 motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_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 similarity index 96% rename from planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp rename to planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 2bcb31ff10969..84ac29d14bdc5 100644 --- a/planning/motion_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 @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace analytical_velocity_planning_utils { @@ -199,13 +199,12 @@ bool calcStopVelocityWithConstantJerkAccLimit( } // for debug - std::stringstream ss; + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Calculate stop velocity."); for (unsigned int i = 0; i < ts.size(); ++i) { - ss << "t: " << ts.at(i) << ", x: " << xs.at(i) << ", v: " << vs.at(i) << ", a: " << as.at(i) - << ", j: " << js.at(i) << std::endl; + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "--- t: %f, x: %f, v: %f, a: %f, j: %f", + ts.at(i), xs.at(i), vs.at(i), as.at(i), js.at(i)); } - RCLCPP_DEBUG( - rclcpp::get_logger("velocity_planning_utils"), "Calculate stop velocity. %s", ss.str().c_str()); const double a_target = 0.0; const double v_margin = 0.3; @@ -354,4 +353,4 @@ double integ_a(double a0, double j0, double t) } } // namespace analytical_velocity_planning_utils -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp similarity index 98% rename from planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 6c2bef3ae08c4..d212cfc0bcedf 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include @@ -27,7 +27,7 @@ #define VERBOSE_TRAJECTORY_VELOCITY false -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -322,7 +322,7 @@ bool JerkFilteredSmoother::apply( output.at(i).acceleration_mps2 = a_stop_decel; } - qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]"); + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); const int status_polish = std::get<2>(result); if (status_polish != 1) { @@ -480,4 +480,4 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( smoother_param_.jerk_filter_ds); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp similarity index 96% rename from planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index d46deeeeb9f10..3c2872f2e58e3 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include @@ -23,7 +23,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -223,7 +223,7 @@ bool L2PseudoJerkSmoother::apply( // v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); // } - qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]"); + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); const auto tf2 = std::chrono::system_clock::now(); const double dt_ms2 = @@ -242,4 +242,4 @@ TrajectoryPoints L2PseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp similarity index 96% rename from planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index e8900b1947454..f434d8d4514ca 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include @@ -23,7 +23,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -236,7 +236,7 @@ bool LinfPseudoJerkSmoother::apply( // v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); // } - qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]"); + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); const auto tf2 = std::chrono::system_clock::now(); const double dt_ms2 = @@ -254,4 +254,4 @@ TrajectoryPoints LinfPseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp similarity index 97% rename from planning/motion_velocity_smoother/src/smoother/smoother_base.cpp rename to planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index bf193b7251382..5704113067244 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/resample.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -26,7 +26,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace @@ -270,4 +270,4 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( return output; } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp similarity index 96% rename from planning/motion_velocity_smoother/src/trajectory_utils.cpp rename to planning/autoware_velocity_smoother/src/trajectory_utils.cpp index acc3673d66925..aff1a0b0e3213 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" @@ -25,7 +25,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { using geometry_msgs::msg::Point; namespace trajectory_utils @@ -202,7 +202,7 @@ std::vector calcTrajectoryCurvatureFrom3Points( } catch (std::exception const & e) { // ...code that handles the error... RCLCPP_WARN( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s", + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "%s", e.what()); if (i > 1) { curvature = k_arr.at(i - 1); // previous curvature @@ -374,7 +374,7 @@ bool calcStopDistWithJerkConstraints( double t1 = calculateTime(a0, 0.0, jerk); if (t1 < 0) { RCLCPP_WARN( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "t1 < 0. unexpected condition."); return false; } else if (t1 < t_threshold) { @@ -399,7 +399,7 @@ bool calcStopDistWithJerkConstraints( stop_dist = x; if (!isValidStopDist(v, a, target_vel, a_target, v_margin, a_margin)) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "valid error. type = " << static_cast(type)); return false; } @@ -416,13 +416,13 @@ bool isValidStopDist( const double a_max = a_target + std::abs(a_margin); if (v_end < v_min || v_max < v_end) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "valid check error! v_target = " << v_target << ", v_end = " << v_end); return false; } if (a_end < a_min || a_max < a_end) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "valid check error! a_target = " << a_target << ", a_end = " << a_end); return false; } @@ -486,7 +486,7 @@ std::optional applyDecelFilterWithJerkConstraint( const double a_margin{0.1}; if (!isValidStopDist(v, a, decel_target_vel, a_target, v_margin, a_margin)) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "validation check error"); return {}; } @@ -562,7 +562,7 @@ std::optional> updateStateWithJerkCon } RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "Invalid jerk profile"); return std::nullopt; } @@ -610,4 +610,4 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes } } // namespace trajectory_utils -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp similarity index 86% rename from planning/motion_velocity_smoother/test/test_smoother_functions.cpp rename to planning/autoware_velocity_smoother/test/test_smoother_functions.cpp index 67196edc17a2e..51ee84f535ca4 100644 --- a/planning/motion_velocity_smoother/test/test_smoother_functions.cpp +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include #include -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using motion_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 = - motion_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/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp similarity index 74% rename from planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp rename to planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index 56813a37941a6..f0145ea5a32a7 100644 --- a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/motion_velocity_smoother_node.hpp" +#include "autoware_velocity_smoother/node.hpp" #include #include @@ -22,37 +22,36 @@ #include -using motion_velocity_smoother::MotionVelocitySmootherNode; +using autoware_velocity_smoother::VelocitySmootherNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); - test_manager->setTrajectorySubscriber("motion_velocity_smoother/output/trajectory"); - test_manager->setTrajectoryInputTopicName("motion_velocity_smoother/input/trajectory"); + test_manager->setTrajectorySubscriber("velocity_smoother/output/trajectory"); + test_manager->setTrajectoryInputTopicName("velocity_smoother/input/trajectory"); test_manager->setOdometryTopicName("/localization/kinematic_state"); return test_manager; } -std::shared_ptr generateNode() +std::shared_ptr generateNode() { auto node_options = rclcpp::NodeOptions{}; node_options.append_parameter_override("algorithm_type", "JerkFiltered"); node_options.append_parameter_override("publish_debug_trajs", false); const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto motion_velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_common.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - "--params-file", - motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", - "--params-file", motion_velocity_smoother_dir + "/config/default_common.param.yaml", - "--params-file", motion_velocity_smoother_dir + "/config/JerkFiltered.param.yaml"}); + "--params-file", velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + "--params-file", velocity_smoother_dir + "/config/default_common.param.yaml", "--params-file", + velocity_smoother_dir + "/config/JerkFiltered.param.yaml"}); - return std::make_shared(node_options); + return std::make_shared(node_options); } void publishMandatoryTopics( @@ -62,11 +61,10 @@ void publishMandatoryTopics( // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); test_manager->publishMaxVelocity( - test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); + test_target_node, "velocity_smoother/input/external_velocity_limit_mps"); test_manager->publishOperationModeState( - test_target_node, "motion_velocity_smoother/input/operation_mode_state"); - test_manager->publishAcceleration( - test_target_node, "motion_velocity_smoother/input/acceleration"); + test_target_node, "velocity_smoother/input/operation_mode_state"); + test_manager->publishAcceleration(test_target_node, "velocity_smoother/input/acceleration"); } TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/README.md b/planning/behavior_path_avoidance_by_lane_change_module/README.md index d91d7116ee056..036de718ccde8 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/README.md +++ b/planning/behavior_path_avoidance_by_lane_change_module/README.md @@ -13,7 +13,7 @@ This module is designed as one of the obstacle avoidance features and generates ## Inner-workings / Algorithms -Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Normal Avoidance Module](../behavior_path_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. +Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Static Object Avoidance Module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. Check that the following conditions are satisfied after the filtering process for the avoidance target. diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp index 8e7d1f67d3157..d7fd8b82bd71e 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp @@ -14,7 +14,7 @@ #ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" namespace behavior_path_planner { diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 54a4a0c70486d..4ef4c0673c0c5 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ +#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_avoidance_module/helper.hpp" #include "behavior_path_lane_change_module/scene.hpp" #include @@ -24,7 +24,7 @@ namespace behavior_path_planner { using AvoidanceDebugData = DebugData; -using helper::avoidance::AvoidanceHelper; +using helper::static_obstacle_avoidance::AvoidanceHelper; class AvoidanceByLaneChange : public NormalLaneChange { diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml index 0f9f3f6dc9d42..84540618e152d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -18,7 +18,7 @@ autoware_cmake eigen3_cmake_module - behavior_path_avoidance_module + autoware_behavior_path_static_obstacle_avoidance_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index a3b28b4d63d06..868b2585170f9 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "behavior_path_avoidance_by_lane_change_module/manager.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -30,7 +30,7 @@ namespace behavior_path_planner void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; // init manager interface diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index d7d7fa8b60513..80ae361ee3ea1 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -14,13 +14,13 @@ #include "behavior_path_avoidance_by_lane_change_module/scene.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" -#include +#include #include #include #include @@ -103,13 +103,14 @@ void AvoidanceByLaneChange::updateSpecialData() if (avoidance_data_.target_objects.empty()) { direction_ = Direction::NONE; } else { - direction_ = utils::avoidance::isOnRight(avoidance_data_.target_objects.front()) + direction_ = utils::static_obstacle_avoidance::isOnRight(avoidance_data_.target_objects.front()) ? Direction::LEFT : Direction::RIGHT; } - utils::avoidance::updateRegisteredObject(registered_objects_, avoidance_data_.target_objects, p); - utils::avoidance::compensateDetectionLost( + utils::static_obstacle_avoidance::updateRegisteredObject( + registered_objects_, avoidance_data_.target_objects, p); + utils::static_obstacle_avoidance::compensateDetectionLost( registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); std::sort( @@ -222,28 +223,29 @@ std::optional AvoidanceByLaneChange::createObjectData( object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0; // Calc envelop polygon. - utils::avoidance::fillObjectEnvelopePolygon( + utils::static_obstacle_avoidance::fillObjectEnvelopePolygon( object_data, registered_objects_, object_closest_pose, p); // calc object centroid. object_data.centroid = return_centroid(object_data.envelope_poly); // Calc moving time. - utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); + utils::static_obstacle_avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 ? Direction::LEFT : Direction::RIGHT; // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_points = - utils::avoidance::calcEnvelopeOverhangDistance(object_data, data.reference_path); + object_data.overhang_points = utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance( + object_data, data.reference_path); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; - utils::avoidance::fillAvoidanceNecessity(object_data, registered_objects_, vehicle_width, p); + utils::static_obstacle_avoidance::fillAvoidanceNecessity( + object_data, registered_objects_, vehicle_width, p); - utils::avoidance::fillLongitudinalAndLengthByClosestEnvelopeFootprint( + utils::static_obstacle_avoidance::fillLongitudinalAndLengthByClosestEnvelopeFootprint( data.reference_path_rough, getEgoPosition(), object_data); return object_data; } @@ -262,7 +264,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ avoidance_helper_->setData(planner_data_); const auto shift_length = avoidance_helper_->getShiftLength( - nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); + nearest_object, utils::static_obstacle_avoidance::isOnRight(nearest_object), avoid_margin); return avoidance_helper_->getMinAvoidanceDistance(shift_length); } diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index f66e944cb7cbc..69ce87aabc7b4 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -67,8 +67,9 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + - "/config/avoidance.param.yaml", + ament_index_cpp::get_package_share_directory( + "autoware_behavior_path_static_obstacle_avoidance_module") + + "/config/static_obstacle_avoidance.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + "/config/avoidance_by_lane_change.param.yaml"}); diff --git a/planning/behavior_path_avoidance_module/plugins.xml b/planning/behavior_path_avoidance_module/plugins.xml deleted file mode 100644 index f25677dad1e9a..0000000000000 --- a/planning/behavior_path_avoidance_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_path_dynamic_avoidance_module/plugins.xml b/planning/behavior_path_dynamic_avoidance_module/plugins.xml deleted file mode 100644 index fd2e1bc4137b7..0000000000000 --- a/planning/behavior_path_dynamic_avoidance_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_path_external_request_lane_change_module/plugins.xml b/planning/behavior_path_external_request_lane_change_module/plugins.xml deleted file mode 100644 index f3cc686837c38..0000000000000 --- a/planning/behavior_path_external_request_lane_change_module/plugins.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp index 4da625a7a9087..58203573d0c35 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp @@ -17,7 +17,7 @@ #include "behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" -#include +#include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index 0926d1010b5c3..9112500e10ac3 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -17,14 +17,14 @@ #include "behavior_path_planner_common/data_manager.hpp" -#include #include +#include #include -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; +using tier4_planning_msgs::msg::PathWithLaneId; namespace behavior_path_planner { diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp index 5ce6d83795069..ae1ee8c66a990 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include 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 e67d458874d17..a76ee5f7529f0 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 @@ -20,7 +20,7 @@ #include -#include +#include #include #include 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 8f456a57e4c78..012af88f4ca70 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 @@ -34,8 +34,8 @@ #include #include -#include -#include +#include +#include #include @@ -50,7 +50,7 @@ namespace behavior_path_planner { -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; using nav_msgs::msg::OccupancyGrid; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index 9700c44445a65..8da00f872f443 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -18,16 +18,16 @@ #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" -#include #include +#include #include #include #include -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; +using tier4_planning_msgs::msg::PathWithLaneId; namespace behavior_path_planner { 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 d0b0aee83e20c..635816f17497f 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 @@ -19,7 +19,7 @@ #include -#include +#include #include #include 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 b6f671cfc94f8..83e817a8d5a45 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 @@ -16,15 +16,16 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ #include "behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include "visualization_msgs/msg/detail/marker_array__struct.hpp" -#include -#include -#include +#include +#include #include #include +#include #include @@ -34,13 +35,14 @@ namespace behavior_path_planner::goal_planner_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +using Polygon2d = tier4_autoware_utils::Polygon2d; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -85,6 +87,10 @@ PathWithLaneId extendPath( const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, const Pose & extend_pose); +std::vector createPathFootPrints( + const PathWithLaneId & path, const double base_to_front, const double base_to_rear, + const double width); + // debug MarkerArray createPullOverAreaMarkerArray( const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, diff --git a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index cbbe5f585dbe2..750efe89de6ab 100644 --- a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -26,7 +26,7 @@ namespace behavior_path_planner { using Point2d = tier4_autoware_utils::Point2d; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const { @@ -93,7 +93,7 @@ PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( constexpr double range_reduce_by{1.0}; // set a reasonable value, 10% - 20% of the // refine_goal_search_radius_range is recommended bool is_valid_path{false}; - autoware_auto_planning_msgs::msg::PathWithLaneId refined_path; + tier4_planning_msgs::msg::PathWithLaneId refined_path; while (goal_search_radius >= 0 && !is_valid_path) { refined_path = utils::refinePathForGoal(goal_search_radius, M_PI * 0.5, path, refined_goal, goal_lane_id); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 9bfef90668e8e..4acc4dce743ad 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1092,7 +1092,7 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const auto original_signal = getPreviousModuleOutput().turn_signal_info; const auto new_signal = calcTurnSignalInfo(); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 8fba7f760ab9d..376adb178708a 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -258,17 +258,39 @@ std::optional ShiftPullOver::generatePullOverPath( road_lane_reference_path_to_shift_end.points.back().point.pose); pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose); - // check if the parking path will leave lanes - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); - const auto combined_drivable = utils::combineDrivableLanes( - expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); - if (lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath())) { + // check if the parking path will leave drivable area and lanes + const bool is_in_parking_lots = std::invoke([&]() -> bool { + const auto & p = planner_data_->parameters; + const auto parking_lot_polygons = + lanelet::utils::query::getAllParkingLots(planner_data_->route_handler->getLaneletMapPtr()); + const auto path_footprints = goal_planner_utils::createPathFootPrints( + pull_over_path.getParkingPath(), p.base_link2front, p.base_link2rear, p.vehicle_width); + const auto is_footprint_in_any_polygon = [&parking_lot_polygons](const auto & footprint) { + return std::any_of( + parking_lot_polygons.begin(), parking_lot_polygons.end(), + [&footprint](const auto & polygon) { + return lanelet::geometry::within(footprint, lanelet::utils::to2D(polygon).basicPolygon()); + }); + }; + return std::all_of( + path_footprints.begin(), path_footprints.end(), + [&is_footprint_in_any_polygon](const auto & footprint) { + return is_footprint_in_any_polygon(footprint); + }); + }); + const bool is_in_lanes = std::invoke([&]() -> bool { + const auto drivable_lanes = + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto & dp = planner_data_->drivable_area_expansion_parameters; + const auto expanded_lanes = utils::expandLanelets( + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + const auto combined_drivable = utils::combineDrivableLanes( + expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); + return !lane_departure_checker_.checkPathWillLeaveLane( + utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath()); + }); + if (!is_in_parking_lots && !is_in_lanes) { return {}; } diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 72ae0143783ea..e94ab74a3bbed 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -433,4 +433,17 @@ PathWithLaneId extendPath( return extendPath(target_path, reference_path, extend_distance); } +std::vector createPathFootPrints( + const PathWithLaneId & path, const double base_to_front, const double base_to_rear, + const double width) +{ + std::vector footprints; + for (const auto & point : path.points) { + const auto & pose = point.point.pose; + footprints.push_back( + tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width)); + } + return footprints; +} + } // namespace behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index 9cbd864be9dbb..09603bedf27df 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -296,7 +296,7 @@ First, we divide the target objects into obstacles in the target lane, obstacles ![object lanes](./images/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_avoidance_module/README.md). +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md). The detection area for the target lane can be expanded beyond its original boundaries to enable detection of objects that are outside the target lane's limits. diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 6e40f880467fd..d442ca2455a4c 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -25,9 +25,9 @@ #include -#include #include #include +#include #include @@ -37,11 +37,11 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeInterface : public SceneModuleInterface { @@ -125,6 +125,18 @@ class LaneChangeInterface : public SceneModuleInterface ModuleStatus setInitState() const override { return ModuleStatus::WAITING_APPROVAL; }; + void updateRTCStatus( + const double start_distance, const double finish_distance, const bool safe, + const uint8_t & state) + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), safe, state, start_distance, finish_distance, clock_->now()); + } + } + } + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 76181f2195a84..b66437f4cff0e 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -23,7 +23,6 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; @@ -35,6 +34,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::Direction; +using tier4_planning_msgs::msg::PathWithLaneId; using utils::path_safety_checker::ExtendedPredictedObjects; class NormalLaneChange : public LaneChangeBase diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 792e8924e5df6..3f241b9d6598d 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -26,9 +26,9 @@ #include #include -#include #include #include +#include #include #include @@ -36,13 +36,13 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::Direction; using tier4_autoware_utils::StopWatch; +using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeBase { diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index 3af4f487a0fa0..4297b4a0ed1cd 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -19,14 +19,14 @@ #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::TurnSignalInfo; +using tier4_planning_msgs::msg::PathWithLaneId; struct LaneChangePath { PathWithLaneId path{}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 2d427213141b4..4bcd61c54b1be 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -25,10 +25,10 @@ #include #include -#include -#include +#include #include #include +#include #include @@ -38,10 +38,9 @@ namespace behavior_path_planner::utils::lane_change { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; @@ -54,6 +53,7 @@ using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using route_handler::Direction; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); diff --git a/planning/behavior_path_lane_change_module/plugins.xml b/planning/behavior_path_lane_change_module/plugins.xml index a598bc8176938..7df36919d8d54 100644 --- a/planning/behavior_path_lane_change_module/plugins.xml +++ b/planning/behavior_path_lane_change_module/plugins.xml @@ -1,6 +1,6 @@ - - + + diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 2567787a3f2e4..1669842117f9f 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -117,13 +117,18 @@ BehaviorModuleOutput LaneChangeInterface::plan() updateSteeringFactorPtr(output); if (module_type_->isAbortState()) { waitApproval(); - removeRTCStatus(); const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus( - candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::ABORTING); } else { clearWaitingApproval(); + const auto path = + assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition()); + updateRTCStatus( + path.start_distance_to_path_change, path.finish_distance_to_path_change, true, + State::RUNNING); } return output; @@ -147,7 +152,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { - removeRTCStatus(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); path_candidate_ = std::make_shared(); return out; } @@ -156,7 +163,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus( - candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, + isExecutionReady(), State::WAITING_FOR_EXECUTION); is_abort_path_approved_ = false; return out; @@ -239,6 +247,9 @@ bool LaneChangeInterface::canTransitFailureState() if (module_type_->isStoppedAtRedTrafficLight()) { log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); module_type_->toCancelState(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } @@ -249,6 +260,9 @@ bool LaneChangeInterface::canTransitFailureState() if (module_type_->isAbleToReturnCurrentLane()) { log_debug_throttled("It's possible to return to current lane. Cancel lane change."); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } } diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2d88a820e0fae..e1c4289b57612 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -204,7 +204,7 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() } // check the priority of turn signals - return getTurnSignalDecider().use_prior_turn_signal( + return getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); } @@ -226,7 +226,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const output.path = abort_path_->path; extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -252,7 +252,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -297,7 +297,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp index 9e44e51d8b72d..708c561bfb629 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -20,9 +20,9 @@ #include #include -#include -#include +#include #include +#include #include #include diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index ab9a0b7176fdb..dd706fb9eab6d 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -57,17 +57,17 @@ namespace behavior_path_planner::utils::lane_change { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using lanelet::ArcCoordinates; +using tier4_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() { @@ -646,8 +646,14 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path) { - const auto start_idx = path.info.shift_line.start_idx; - const auto end_idx = path.info.shift_line.end_idx; + if (path.shifted_path.shift_length.empty()) { + return 0.0; + } + + const auto start_idx = + std::min(path.info.shift_line.start_idx, path.shifted_path.shift_length.size() - 1); + const auto end_idx = + std::min(path.info.shift_line.end_idx, path.shifted_path.shift_length.size() - 1); return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index beaa1ae6c5263..4d348e6c9ab58 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -24,17 +24,17 @@ Essentially, the module has three primary responsibilities: Behavior Path Planner has following scene modules -| Name | Description | Details | -| :----------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :----------------------------------------------------------------- | -| Lane Following | this module generates reference path from lanelet centerline. | LINK | -| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_module/README.md) | -| Dynamic Avoidance | WIP | [LINK](../behavior_path_dynamic_avoidance_module/README.md) | -| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | -| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../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) | +| Name | Description | Details | +| :------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------------- | +| Lane Following | this module generates reference path from lanelet centerline. | LINK | +| Static Obstacle Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) | +| Dynamic Obstacle Avoidance | WIP | [LINK](../autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md) | +| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | +| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../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) | !!! Note @@ -89,32 +89,32 @@ The Planner Manager's responsibilities include: ### Input -| Name | Required? | Type | Description | -| :---------------------------- | :-------: | :----------------------------------------------------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | -| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | -| ~/input/objects | ○ | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | -| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | -| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficSignalArray` | traffic signals information from the perception module | -| ~/input/vector_map | ○ | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map information. | -| ~/input/route | ○ | `autoware_auto_mapping_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | -| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | -| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/operation-mode.md) | +| Name | Required? | Type | Description | +| :---------------------------- | :-------: | :------------------------------------------------------ | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | +| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | +| ~/input/objects | ○ | `autoware_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | +| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | +| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | traffic signals information from the perception module | +| ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | vector map information. | +| ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | +| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | +| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | +| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/operation-mode.md) | - ○ Mandatory: Planning Module would not work if anyone of this is not present. - △ Optional: Some module would not work, but Planning Module can still be operated. ### Output -| Name | Type | Description | QoS Durability | -| :---------------------------- | :------------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | -| ~/output/path | `autoware_auto_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | -| ~/output/turn_indicators_cmd | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | `volatile` | -| ~/output/hazard_lights_cmd | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command. | `volatile` | -| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | output modified goal commands. | `transient_local` | -| ~/output/stop_reasons | `tier4_planning_msgs::msg::StopReasonArray` | describe the reason for ego vehicle stop | `volatile` | -| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | +| Name | Type | Description | QoS Durability | +| :---------------------------- | :-------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | +| ~/output/path | `tier4_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | +| ~/output/turn_indicators_cmd | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | `volatile` | +| ~/output/hazard_lights_cmd | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command. | `volatile` | +| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | output modified goal commands. | `transient_local` | +| ~/output/stop_reasons | `tier4_planning_msgs::msg::StopReasonArray` | describe the reason for ego vehicle stop | `volatile` | +| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | ### Debug @@ -125,8 +125,8 @@ The Planner Manager's responsibilities include: | ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | shows maximum static drivable area. | `volatile` | | ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | | ~/debug/bound | `visualization_msgs::msg::MarkerArray` | debug for static drivable area | `volatile` | -| ~/planning/path_candidate/\* | `autoware_auto_planning_msgs::msg::Path` | the path before approval. | `volatile` | -| ~/planning/path_reference/\* | `autoware_auto_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | +| ~/planning/path_candidate/\* | `autoware_planning_msgs::msg::Path` | the path before approval. | `volatile` | +| ~/planning/path_reference/\* | `autoware_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | !!! note @@ -138,19 +138,19 @@ Enabling and disabling the modules in the behavior path planner is primarily man The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the behavior path planner's modules or features. For example: -- `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. +- `launch_static_obstacle_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. !!! note Click [here](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/preset/default_preset.yaml) to view the `default_preset.yaml`. -The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `avoidance.enable_module` in +The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `static_obstacle_avoidance.enable_module` in ```xml - + ``` -corresponds to launch_avoidance_module from `default_preset.yaml`. +corresponds to launch_static_obstacle_avoidance_module from `default_preset.yaml`. Therefore, to enable or disable a module, simply set the corresponding module in `default_preset.yaml` to `true` or `false`. These changes will be applied upon the next launch of Autoware. @@ -220,7 +220,7 @@ Large vehicles require much more space, which sometimes causes them to veer out ## Generating Turn Signal -The Behavior Path Planner module uses the `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` to output turn signal commands (see [TurnIndicatorsCommand.idl](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl)). The system evaluates the driving context and determines when to activate turn signals based on its maneuver planning—like turning, lane changing, or obstacle avoidance. +The Behavior Path Planner module uses the `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` to output turn signal commands (see [TurnIndicatorsCommand.idl](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg)). The system evaluates the driving context and determines when to activate turn signals based on its maneuver planning—like turning, lane changing, or obstacle avoidance. Within this framework, the system differentiates between **desired** and **required** blinker activations. **Desired** activations are those recommended by traffic laws for typical driving scenarios, such as signaling before a lane change or turn. **Required** activations are those that are deemed mandatory for safety reasons, like signaling an abrupt lane change to avoid an obstacle. @@ -245,12 +245,12 @@ behavior_path_planner ├── behavior_path_planner.param.yaml ├── drivable_area_expansion.param.yaml ├── scene_module_manager.param.yaml -├── avoidance -│ └── avoidance.param.yaml +├── static_obstacle_avoidance +│ └── static_obstacle_avoidance.param.yaml ├── avoidance_by_lc │ └── avoidance_by_lc.param.yaml -├── dynamic_avoidance -│ └── dynamic_avoidance.param.yaml +├── dynamic_obstacle_avoidance +│ └── dynamic_obstacle_avoidance.param.yaml ├── goal_planner │ └── goal_planner.param.yaml ├── lane_change diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 2c181489ba4c2..e5c3a76fd1c45 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -17,6 +17,7 @@ turn_signal_minimum_search_distance: 10.0 turn_signal_search_time: 3.0 turn_signal_shift_length_threshold: 0.3 + turn_signal_remaining_shift_length_threshold: 0.1 turn_signal_on_swerving: true enable_akima_spline_first: false diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index c2b6babc402a2..6cf8719b2ef83 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -59,7 +59,7 @@ priority: 1 max_module_size: 1 - avoidance: + static_obstacle_avoidance: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false @@ -75,7 +75,7 @@ priority: 3 max_module_size: 1 - dynamic_avoidance: + dynamic_obstacle_avoidance: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md index 7188c03bd2604..5ffdcafdb2497 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -119,8 +119,8 @@ Scene modules receives necessary data and RTC command, and outputs candidate pat | IN | `tier4_planning_msgs::srv::CooperateCommands` | contains approval data for scene module's path modification. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md)) | | OUT | `behavior_path_planner::BehaviorModuleOutput` | contains modified path, turn signal information, etc... | | OUT | `tier4_planning_msgs::msg::CooperateStatus` | contains RTC cooperate status. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md)) | -| OUT | `autoware_auto_planning_msgs::msg::Path` | candidate path output by a module that has not received approval for path change. when it approved, the ego's following path is switched to this path. (just for visualization) | -| OUT | `autoware_auto_planning_msgs::msg::Path` | reference path generated from the centerline of the lane the ego is going to follow. (just for visualization) | +| OUT | `autoware_planning_msgs::msg::Path` | candidate path output by a module that has not received approval for path change. when it approved, the ego's following path is switched to this path. (just for visualization) | +| OUT | `autoware_planning_msgs::msg::Path` | reference path generated from the centerline of the lane the ego is going to follow. (just for visualization) | | OUT | `visualization_msgs::msg::MarkerArray` | virtual wall, debug info, etc... | Scene modules running on the manager are stored on the **candidate modules stack** or **approved modules stack** depending on the condition whether the path modification has been approved or not. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 5a49f4d9ab66e..3be680aa06662 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -24,19 +24,19 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include #include +#include #include +#include +#include #include #include #include #include #include +#include #include #include #include @@ -52,22 +52,22 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::StopReasonArray; @@ -87,14 +87,14 @@ class BehaviorPathPlannerNode : public rclcpp::Node private: rclcpp::Subscription::SharedPtr route_subscriber_; - rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr vector_map_subscriber_; rclcpp::Subscription::SharedPtr velocity_subscriber_; rclcpp::Subscription::SharedPtr acceleration_subscriber_; rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; rclcpp::Subscription::SharedPtr costmap_subscriber_; - rclcpp::Subscription::SharedPtr traffic_signals_subscriber_; + rclcpp::Subscription::SharedPtr traffic_signals_subscriber_; rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; rclcpp::Subscription::SharedPtr operation_mode_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; @@ -118,7 +118,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::unique_ptr steering_factor_interface_ptr_; Scenario::SharedPtr current_scenario_{nullptr}; - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr_{nullptr}; LaneletRoute::ConstSharedPtr route_ptr_{nullptr}; bool has_received_map_{false}; bool has_received_route_{false}; @@ -140,8 +140,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); void onCostMap(const OccupancyGrid::ConstSharedPtr msg); - void onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg); - void onMap(const HADMapBin::ConstSharedPtr map_msg); + void onTrafficSignals(const TrafficLightGroupArray::ConstSharedPtr msg); + void onMap(const LaneletMapBin::ConstSharedPtr map_msg); void onRoute(const LaneletRoute::ConstSharedPtr route_msg); void onOperationMode(const OperationModeState::ConstSharedPtr msg); void onLateralOffset(const LateralOffset::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 877a8c62cbb63..4eabe5f90d877 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include @@ -39,8 +39,8 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::StopWatch; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopReasonArray; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 14220c09880de..2f8f0ad0646a6 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -37,13 +37,12 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 - autoware_auto_vehicle_msgs autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_msgs behavior_path_planner_common + behaviortree_cpp_v3 freespace_planning_algorithms frenet_planner geometry_msgs diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index b56d1a207f76d..f50cb94f436e6 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -96,7 +96,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1), createSubscriptionOptions(this)); traffic_signals_subscriber_ = - this->create_subscription( + this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorPathPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); lateral_offset_subscriber_ = this->create_subscription( @@ -119,7 +119,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod createSubscriptionOptions(this)); // route_handler - vector_map_subscriber_ = create_subscription( + vector_map_subscriber_ = create_subscription( "~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1), createSubscriptionOptions(this)); route_subscriber_ = create_subscription( @@ -229,7 +229,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() // size of a drivable area. // The drivable area has to cover not the base link but the vehicle itself. Therefore // rear_overhang must be added to backward_path_length. In addition, because of the - // calculation of the drivable area in the obstacle_avoidance_planner package, the drivable + // calculation of the drivable area in the autoware_path_optimizer package, the drivable // area has to be a little longer than the backward_path_length parameter by adding // min_backward_offset. constexpr double min_backward_offset = 1.0; @@ -260,6 +260,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_signal_search_time = declare_parameter("turn_signal_search_time"); p.turn_signal_shift_length_threshold = declare_parameter("turn_signal_shift_length_threshold"); + p.turn_signal_remaining_shift_length_threshold = + declare_parameter("turn_signal_remaining_shift_length_threshold"); p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving"); p.enable_akima_spline_first = declare_parameter("enable_akima_spline_first"); @@ -337,7 +339,7 @@ void BehaviorPathPlannerNode::run() } // check for map update - HADMapBin::ConstSharedPtr map_ptr{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr{nullptr}; { std::lock_guard lk_map(mutex_map_); // for has_received_map_ and map_ptr_ if (has_received_map_) { @@ -709,8 +711,8 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = motion_utils::convertToPath( - *path_candidate_ptr); + output = + motion_utils::convertToPath(*path_candidate_ptr); // header is replaced by the input one, so it is substituted again output.header = planner_data->route_handler->getRouteHeader(); output.header.stamp = this->now(); @@ -788,19 +790,19 @@ void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg) const std::lock_guard lock(mutex_pd_); planner_data_->costmap = msg; } -void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg) +void BehaviorPathPlannerNode::onTrafficSignals(const TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_pd_); planner_data_->traffic_light_id_map.clear(); - for (const auto & signal : msg->signals) { + for (const auto & signal : msg->traffic_light_groups) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_->traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + planner_data_->traffic_light_id_map[signal.traffic_light_group_id] = traffic_signal; } } -void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg) +void BehaviorPathPlannerNode::onMap(const LaneletMapBin::ConstSharedPtr msg) { const std::lock_guard lock(mutex_map_); map_ptr_ = msg; diff --git a/planning/behavior_path_planner/test/input.cpp b/planning/behavior_path_planner/test/input.cpp index c2735abd3e932..ba87af1525692 100644 --- a/planning/behavior_path_planner/test/input.cpp +++ b/planning/behavior_path_planner/test/input.cpp @@ -15,7 +15,7 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Twist; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample) diff --git a/planning/behavior_path_planner/test/input.hpp b/planning/behavior_path_planner/test/input.hpp index ededb32f78be0..33e538156990b 100644 --- a/planning/behavior_path_planner/test/input.hpp +++ b/planning/behavior_path_planner/test/input.hpp @@ -17,21 +17,21 @@ #endif // INPUT_HPP_ -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "tier4_planning_msgs/msg/path_point_with_lane_id.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample); diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md index bfc22bacc8ede..4302b74dcf2c6 100644 --- a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md @@ -20,14 +20,15 @@ Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in c ## Parameters for turn signal decider -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------- | :--- | :----- | :--------------------------------------------------------------------------- | :------------ | -| turn_signal_intersection_search_distance | [m] | double | constant search distance to decide activation of blinkers at intersections | 30 | -| turn_signal_intersection_angle_threshold_degree | deg | double | angle threshold to determined the end point of intersection required section | 15 | -| turn_signal_minimum_search_distance | [m] | double | minimum search distance for avoidance and lane change | 10 | -| turn_signal_search_time | [s] | double | search time for to decide activation of blinkers | 3.0 | -| turn_signal_shift_length_threshold | [m] | double | shift length threshold to decide activation of blinkers | 0.3 | -| turn_signal_on_swerving | [-] | bool | flag to activate blinkers when swerving | true | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------ | :------------ | +| turn_signal_intersection_search_distance | [m] | double | constant search distance to decide activation of blinkers at intersections | 30 | +| turn_signal_intersection_angle_threshold_degree | deg | double | angle threshold to determined the end point of intersection required section | 15 | +| turn_signal_minimum_search_distance | [m] | double | minimum search distance for avoidance and lane change | 10 | +| turn_signal_search_time | [s] | double | search time for to decide activation of blinkers | 3.0 | +| turn_signal_shift_length_threshold | [m] | double | shift length threshold to decide activation of blinkers | 0.3 | +| turn_signal_remaining_shift_length_threshold | [m] | double | When the ego's current shift length minus its end shift length is less than this threshold, the turn signal will be turned off. | 0.1 | +| turn_signal_on_swerving | [-] | bool | flag to activate blinkers when swerving | true | Note that the default values for `turn_signal_intersection_search_distance` and `turn_signal_search_time` is strictly followed by [Japanese Road Traffic Laws](https://www.japaneselawtranslation.go.jp/ja/laws/view/2962). So if your country does not allow to use these default values, you should change these values in configuration files. diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 09f3c2c66bcd4..90a2bf58a4e13 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -27,12 +27,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include #include +#include +#include #include #include #include @@ -40,6 +39,7 @@ #include #include #include +#include #include #include @@ -51,19 +51,19 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightGroup; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using route_handler::RouteHandler; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; using PlanResult = PathWithLaneId::SharedPtr; using lanelet::TrafficLight; using tier4_planning_msgs::msg::VelocityLimit; @@ -72,7 +72,7 @@ using unique_identifier_msgs::msg::UUID; struct TrafficSignalStamped { builtin_interfaces::msg::Time stamp; - TrafficSignal signal; + TrafficLightGroup signal; }; struct BoolStamped diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index bd7949a83a1d7..c752e2a7f813d 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -36,8 +36,8 @@ #include #include -#include #include +#include #include #include #include @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -57,7 +58,6 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::SteeringFactor; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; @@ -65,6 +65,7 @@ using steering_factor_interface::SteeringFactorInterface; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; @@ -186,6 +187,10 @@ class SceneModuleInterface { RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); + if (getCurrentStatus() == ModuleStatus::SUCCESS) { + updateRTCStatusForSuccess(); + } + clearWaitingApproval(); unlockNewModuleLaunch(); unlockOutputPath(); @@ -499,9 +504,23 @@ class SceneModuleInterface } } + void updateRTCStatusForSuccess() + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + if (ptr->isRegistered(uuid_map_.at(module_name))) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), true, State::SUCCEEDED, + std::numeric_limits::lowest(), std::numeric_limits::lowest(), + clock_->now()); + } + } + } + } + void setObjectsOfInterestData( const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) + const autoware_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) { for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) { if (ptr) { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp index ffc5daa7aa2ff..1bce30b18edd7 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp @@ -21,7 +21,7 @@ namespace behavior_path_planner { // Forward Declaration -class AvoidanceModule; +class StaticObstacleAvoidanceModule; class AvoidanceByLCModule; class ExternalRequestLaneChangeModule; class LaneChangeInterface; @@ -35,7 +35,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; class SceneModuleVisitor { public: - void visitAvoidanceModule(const AvoidanceModule * module) const; + void visitAvoidanceModule(const StaticObstacleAvoidanceModule * module) const; std::shared_ptr getAvoidanceModuleDebugMsg() const; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp index 6426f16a44259..205afb81b460b 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp @@ -20,10 +20,10 @@ #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -34,9 +34,8 @@ namespace marker_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::DrivableLanes; using behavior_path_planner::ShiftLineArray; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; @@ -50,6 +49,7 @@ using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 1241f98daa747..dbe27d856bc40 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -54,6 +54,7 @@ struct BehaviorPathPlannerParameters double turn_signal_search_time; double turn_signal_minimum_search_distance; double turn_signal_shift_length_threshold; + double turn_signal_remaining_shift_length_threshold; bool turn_signal_on_swerving; bool enable_akima_spline_first; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index 29fe05775739e..7e335b09e4fd3 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -24,10 +24,10 @@ #include #include -#include -#include -#include +#include +#include #include +#include #include @@ -42,13 +42,13 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using route_handler::RouteHandler; +using tier4_planning_msgs::msg::PathWithLaneId; const std::map g_signal_map = { {"left", TurnIndicatorsCommand::ENABLE_LEFT}, @@ -64,6 +64,17 @@ struct TurnSignalInfo hazard_signal.command = HazardLightsCommand::NO_COMMAND; } + TurnSignalInfo(const Pose & start, const Pose & end) + { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + hazard_signal.command = HazardLightsCommand::NO_COMMAND; + + desired_start_point = start; + desired_end_point = end; + required_start_point = start; + required_end_point = end; + } + // desired turn signal TurnIndicatorsCommand turn_signal; HazardLightsCommand hazard_signal; @@ -93,6 +104,11 @@ class TurnSignalDecider const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info, const double nearest_dist_threshold, const double nearest_yaw_threshold); + TurnSignalInfo overwrite_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const; + TurnSignalInfo use_prior_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index 2a4343e2c3f6a..980da88207f6d 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include namespace drivable_area_expansion @@ -43,7 +43,7 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths MultiPolygon2d create_object_footprints( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const autoware_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index e438105c6645e..aa9f9d3c1b081 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -17,10 +17,10 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include -#include -#include +#include +#include #include +#include #include @@ -29,12 +29,12 @@ namespace drivable_area_expansion { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::MultiLineString2d; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 5b1e9b4b4c419..9d18caaa7bd28 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -17,9 +17,9 @@ #include -#include #include #include +#include #include @@ -99,8 +99,7 @@ class OccupancyGridBasedCollisionDetector bool hasObstacleOnPath( const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const; bool hasObstacleOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const bool check_out_of_range) const; + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } bool detectCollision(const IndexXYT & base_index, const bool check_out_of_range) const; virtual ~OccupancyGridBasedCollisionDetector() = default; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 74f6b843803df..5bc360d3e789a 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -17,13 +17,13 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include +#include #include namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 1489dd1beff07..a70862756f8e9 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -20,10 +20,10 @@ #include -#include -#include +#include #include #include +#include #include @@ -33,10 +33,10 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; struct ParallelParkingParameters { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index a0b2d0afa0fa8..ec5fdb42ead03 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -18,9 +18,9 @@ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include -#include +#include +#include +#include #include @@ -31,8 +31,8 @@ namespace behavior_path_planner::utils::path_safety_checker::filter { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using tier4_planning_msgs::msg::PathPointWithLaneId; bool velocity_filter( const PredictedObject & object, double velocity_threshold, double max_velocity); @@ -47,9 +47,9 @@ bool position_filter( namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using tier4_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters objects based on object centroid position. diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index d83c748f214b4..45e3c9e5e9908 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -31,7 +31,7 @@ namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using tier4_autoware_utils::Polygon2d; @@ -77,8 +77,8 @@ struct ExtendedPredictedObject geometry_msgs::msg::PoseWithCovariance initial_pose; geometry_msgs::msg::TwistWithCovariance initial_twist; geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_auto_perception_msgs::msg::Shape shape; - std::vector classification; + autoware_perception_msgs::msg::Shape shape; + std::vector classification; std::vector predicted_paths; ExtendedPredictedObject() = default; @@ -230,7 +230,7 @@ struct CollisionCheckDebug std::vector obj_predicted_path; ///< object's predicted path. Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. - autoware_auto_perception_msgs::msg::Shape obj_shape; ///< object's shape. + autoware_perception_msgs::msg::Shape obj_shape; ///< object's shape. }; using CollisionCheckDebugPair = std::pair; using CollisionCheckDebugMap = diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index c4cfed01450b7..af596ddc293cd 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -31,9 +31,9 @@ namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 880e55b64d170..6651647f8d262 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -19,8 +19,8 @@ #include #include -#include #include +#include #include #include @@ -29,11 +29,11 @@ #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using tier4_autoware_utils::generateUUID; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; struct ShiftLine diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp index f109bb2cbb213..67476a587decf 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp @@ -21,10 +21,10 @@ #include #include -#include -#include -#include +#include +#include #include +#include #include @@ -35,11 +35,11 @@ namespace behavior_path_planner::utils { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_planning_msgs::msg::Path; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathWithLaneId; std::vector calcPathArcLengthArray( const PathWithLaneId & path, const size_t start = 0, diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp index 62fd87bbd91bc..84896cc72daa3 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -33,8 +33,8 @@ namespace behavior_path_planner::utils::traffic_light { -using autoware_perception_msgs::msg::TrafficSignal; -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; using geometry_msgs::msg::Pose; /** diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index b61ef0579cab0..f4296073eb413 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -21,16 +21,16 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include +#include +#include #include @@ -41,19 +41,19 @@ namespace behavior_path_planner::utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Vector3; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; struct PolygonPoint { diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index 7bc40974b9a62..ce0f2e716ace3 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -43,10 +43,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 autoware_perception_msgs + autoware_planning_msgs freespace_planning_algorithms geometry_msgs interpolation diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 21937aa76da29..146fb03cc00f2 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -392,6 +392,40 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( return intersection_signal_info.turn_signal; } +TurnSignalInfo TurnSignalDecider::overwrite_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const +{ + if (original_signal.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND) { + return new_signal; + } + + if (original_signal.turn_signal.command == TurnIndicatorsCommand::DISABLE) { + return new_signal; + } + + const auto get_distance = [&](const Pose & input_point) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point.position, + nearest_seg_idx) - + base_link2front_; + }; + + const auto & original_desired_end_point = original_signal.desired_end_point; + const auto & new_desired_start_point = new_signal.desired_start_point; + + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_new_desired_start = get_distance(new_desired_start_point); + if (dist_to_new_desired_start > dist_to_original_desired_end) { + return original_signal; + } + + return new_signal; +} + TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, @@ -615,7 +649,8 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check, const bool is_pull_out) const { - constexpr double THRESHOLD = 0.1; + using tier4_autoware_utils::getPose; + const auto & p = parameters; const auto & rh = route_handler; const auto & ego_pose = self_odometry->pose.pose; @@ -668,14 +703,19 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const auto relative_shift_length = end_shift_length - start_shift_length; + const auto p_path_start = getPose(path.path.points.front()); + const auto p_path_end = getPose(path.path.points.back()); + // If shift length is shorter than the threshold, it does not need to turn on blinkers if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // If the vehicle does not shift anymore, we turn off the blinker - if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) { - return std::make_pair(TurnSignalInfo{}, true); + if ( + std::fabs(end_shift_length - current_shift_length) < + p.turn_signal_remaining_shift_length_threshold) { + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } const auto get_command = [](const auto & shift_length) { @@ -690,7 +730,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( p.vehicle_info.max_longitudinal_offset_m; if (signal_prepare_distance < ego_front_to_shift_start) { - return std::make_pair(TurnSignalInfo{}, false); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false); } const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; @@ -707,13 +747,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( turn_signal_info.turn_signal.command = get_command(relative_shift_length); if (!p.turn_signal_on_swerving) { - return std::make_pair(turn_signal_info, false); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false); } lanelet::ConstLanelet lanelet; const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start; if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); @@ -728,13 +768,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( !is_pull_out && !existShiftSideLane( start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // Check if the ego will cross lane bounds. // Note that pull out requires blinkers, even if the ego does not cross lane bounds if (!is_pull_out && !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // If the ego has stopped and its close to completing its shift, turn off the blinkers @@ -743,7 +783,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( if (isNearEndOfShift( start_shift_length, end_shift_length, ego_pose.position, current_lanelets, p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } } diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index 787af681bf69b..8565d737d5762 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -41,7 +41,7 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 } MultiPolygon2d create_object_footprints( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const autoware_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { MultiPolygon2d footprints; diff --git a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 1aab58ac2ff6e..7dae0fb32c79f 100644 --- a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -110,6 +110,14 @@ void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyG } } +static IndexXY position2index( + const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Point & position_local) +{ + const int index_x = position_local.x / costmap.info.resolution; + const int index_y = position_local.y / costmap.info.resolution; + return {index_x, index_y}; +} + void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( int theta_index, std::vector & indexes_2d) { @@ -131,12 +139,11 @@ void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( const double offset_x = std::cos(base_theta) * x - std::sin(base_theta) * y; const double offset_y = std::sin(base_theta) * x + std::cos(base_theta) * y; - geometry_msgs::msg::Pose pose_local; - pose_local.position.x = base_pose.position.x + offset_x; - pose_local.position.y = base_pose.position.y + offset_y; + geometry_msgs::msg::Point position_local; + position_local.x = base_pose.position.x + offset_x; + position_local.y = base_pose.position.y + offset_y; - const auto index = pose2index(costmap_, pose_local, param_.theta_size); - const auto index_2d = IndexXY{index.x, index.y}; + const auto index_2d = position2index(costmap_, position_local); indexes_2d.push_back(index_2d); }; @@ -193,8 +200,7 @@ bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( } bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const bool check_out_of_range) const + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const { for (const auto & p : path.points) { const auto pose_local = global2local(costmap_, p.point.pose); diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 9400abd20b984..90790f61db1a0 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -31,7 +31,6 @@ #include #include -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::utils::getArcCoordinates; @@ -40,6 +39,7 @@ using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPoint; using tier4_autoware_utils::normalizeRadian; using tier4_autoware_utils::transformPose; +using tier4_planning_msgs::msg::PathWithLaneId; namespace behavior_path_planner { diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index cdb4b778d3ab3..3abd9b6eb0c05 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -398,7 +398,7 @@ TargetObjectsOnLane createTargetObjectsOnLane( bool isTargetObjectType( const PredictedObject & object, const ObjectTypesToCheck & target_object_types) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; const auto t = utils::getHighestProbLabel(object.classification); return ( (t == ObjectClassification::CAR && target_object_types.check_car) || diff --git a/planning/behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner_common/src/utils/path_utils.cpp index d11bc5a68e9b5..ed583a464634c 100644 --- a/planning/behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_utils.cpp @@ -67,7 +67,7 @@ PathWithLaneId resamplePathWithSpline( return path; } - std::vector transformed_path(path.points.size()); + std::vector transformed_path(path.points.size()); for (size_t i = 0; i < path.points.size(); ++i) { transformed_path.at(i) = path.points.at(i).point; } diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index d674f7770ac67..ea5d07b1697eb 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -40,7 +40,7 @@ namespace { double calcInterpolatedZ( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const tier4_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Point target_pos, const size_t seg_idx) { const double closest_to_target_dist = motion_utils::calcSignedArcLength( @@ -58,7 +58,7 @@ double calcInterpolatedZ( } double calcInterpolatedVelocity( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) + const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) { const double seg_dist = motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); @@ -71,8 +71,8 @@ double calcInterpolatedVelocity( namespace behavior_path_planner::utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::PoseWithCovarianceStamped; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; @@ -272,9 +272,8 @@ bool exists(std::vector vec, T element) } std::optional findIndexOutOfGoalSearchRange( - const std::vector & points, - const Pose & goal, const int64_t goal_lane_id, - const double max_dist = std::numeric_limits::max()) + const std::vector & points, const Pose & goal, + const int64_t goal_lane_id, const double max_dist = std::numeric_limits::max()) { if (points.empty()) { return std::nullopt; diff --git a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp index 77728cc87604d..f244845679cea 100644 --- a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp @@ -176,7 +176,7 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area) { drivable_area_expansion::DrivableAreaExpansionParameters params; drivable_area_expansion::PredictedObjects dynamic_objects; - autoware_auto_mapping_msgs::msg::HADMapBin map; + autoware_map_msgs::msg::LaneletMapBin map; lanelet::LaneletMapPtr empty_lanelet_map_ptr = std::make_shared(); lanelet::utils::conversion::toBinMsg(empty_lanelet_map_ptr, &map); route_handler::RouteHandler route_handler(map); diff --git a/planning/behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner_common/test/test_safety_check.cpp index f85f52bae53ad..e2dd1ade110d0 100644 --- a/planning/behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner_common/test/test_safety_check.cpp @@ -27,7 +27,7 @@ constexpr double epsilon = 1e-6; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::Shape; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -143,7 +143,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + shape.type = autoware_perception_msgs::msg::Shape::POLYGON; shape.footprint.points.resize(5); shape.footprint.points.at(0).x = 3.0; shape.footprint.points.at(0).y = 0.0; diff --git a/planning/behavior_path_planner_common/test/test_turn_signal.cpp b/planning/behavior_path_planner_common/test/test_turn_signal.cpp index 89101ca5c1ed7..2a8ea55f8d3c3 100644 --- a/planning/behavior_path_planner_common/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner_common/test/test_turn_signal.cpp @@ -16,16 +16,15 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" #include #include #include -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_planning_msgs::msg::PathPoint; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using behavior_path_planner::PathWithLaneId; using behavior_path_planner::Pose; using behavior_path_planner::TurnSignalDecider; @@ -34,6 +33,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Twist; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromYaw; +using tier4_planning_msgs::msg::PathPointWithLaneId; constexpr double nearest_dist_threshold = 5.0; constexpr double nearest_yaw_threshold = M_PI / 3.0; 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 6fac6216eb740..1683d4926c71b 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 @@ -41,8 +41,8 @@ #include "tier4_autoware_utils/system/stop_watch.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include "tier4_planning_msgs/msg/lateral_offset.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include #include @@ -58,7 +58,7 @@ #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; struct SamplingPlannerData { // input diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml index f016cb6de1e7c..c09b1e237d3cf 100644 --- a/planning/behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -13,12 +13,10 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 - autoware_auto_vehicle_msgs autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_msgs behavior_path_planner_common bezier_sampler frenet_planner diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp index 08c2345df5814..c4e1c0f3c811b 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp @@ -21,8 +21,8 @@ #include -#include #include +#include #include #include @@ -32,10 +32,10 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; class SideShiftModule : public SceneModuleInterface { diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp index 210dd4e8a8d2a..f53bd42363b55 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp @@ -15,17 +15,17 @@ #ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ #define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ -#include #include #include #include +#include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; +using tier4_planning_msgs::msg::PathWithLaneId; void setOrientation(PathWithLaneId * path); diff --git a/planning/behavior_path_side_shift_module/package.xml b/planning/behavior_path_side_shift_module/package.xml index 5c421b04c0e39..c1f3b3f3cb921 100644 --- a/planning/behavior_path_side_shift_module/package.xml +++ b/planning/behavior_path_side_shift_module/package.xml @@ -18,7 +18,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_path_planner behavior_path_planner_common geometry_msgs diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index ff0550e4d4867..ab2f0460b3fcb 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -91,36 +91,66 @@ The `StartPlannerModule` is designed to initiate its execution based on specific ### **End Conditions** -The `StartPlannerModule` terminates if the pull out / freespace maneuver has been completed. The `canTransitSuccessState` function assesses these conditions to decide if the module should terminate its execution. +The `StartPlannerModule` terminates when specific conditions are met, depending on the type of planner being used. The `canTransitSuccessState` function determines whether the module should transition to the success state based on the following criteria: -```plantuml -@startuml -start -:Start hasFinishedPullOut(); +#### When the Freespace Planner is active -if (status_.driving_forward && status_.found_pull_out_path) then (yes) +- If the end point of the freespace path is reached, the module transitions to the success state. - if (status_.planner_type == FREESPACE) then (yes) - :Calculate distance\nto pull_out_path.end_pose; - if (distance < th_arrived_distance) then (yes) - :return true;\n(Terminate module); - else (no) - :return false;\n(Continue running); - endif - else (no) - :Calculate arclength for\ncurrent_pose and pull_out_path.end_pose; - if (arclength_current - arclength_pull_out_end > offset) then (yes) - :return true;\n(Terminate module); - else (no) - :return false;\n(Continue running); - endif - endif +#### When any other type of planner is active + +The transition to the success state is determined by the following conditions: + +- If a reverse path is being generated or the search for a pull out path fails: + - The module does not transition to the success state. +- If the end point of the pull out path's shift section is reached: + - The module transitions to the success state. + +The flowchart below illustrates the decision-making process in the `canTransitSuccessState` function: +```plantuml +@startuml +@startuml +skinparam ActivityBackgroundColor #white +skinparam ActivityBorderColor #black +skinparam ActivityBorderThickness 1 +skinparam ActivityArrowColor #black +skinparam ActivityArrowThickness 1 +skinparam ActivityStartColor #black +skinparam ActivityEndColor #black +skinparam ActivityDiamondBackgroundColor #white +skinparam ActivityDiamondBorderColor #black +skinparam ActivityDiamondFontColor #black +partition canTransitSuccessState() { +start +if (planner type is FREESPACE?) then (yes) +if (Has reached freespace end?) then (yes) +#FF006C:true; +stop else (no) - :return false;\n(Continue running); +:false; +stop endif - +else (no) +if (driving is forward?) then (yes) +if (pull out path is found?) then (yes) +if (Has reached pull out end?) then (yes) +#FF006C:true; stop +else (no) +:false; +stop +endif +else (no) +:false; +stop +endif +else (no) +:false; +stop +endif +endif +} @enduml ``` @@ -464,14 +494,15 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 #### parameters for geometric pull out -| Name | Unit | Type | Description | Default value | -| :-------------------------- | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | -| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | -| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | -| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | -| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | -| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | +| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | +| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | +| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | +| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | +| lane_departure_check_expansion_margin | [m] | double | margin to expand the ego vehicle footprint when doing lane departure checks | 0.0 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | ## **backward pull out start point search** diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 46469de68149e..7de76c28cef5c 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -40,6 +40,7 @@ geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 lane_departure_margin: 0.2 + lane_departure_check_expansion_margin: 0.0 backward_velocity: -1.0 pull_out_max_steer_angle: 0.26 # 15deg # search start pose backward diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 0e85c1d0e76b3..4e6177cf1e1aa 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -29,7 +29,7 @@ namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; @@ -71,6 +71,7 @@ struct StartPlannerParameters behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck object_types_to_check_for_path_generation{}; double center_line_path_interval{0.0}; + double lane_departure_check_expansion_margin{0.0}; // shift pull out bool enable_shift_pull_out{false}; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp index 75e55d6eab410..561ef337fa68c 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp index 138aaaddd6981..68c25a7f5d39a 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -21,7 +21,7 @@ #include -#include +#include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp index 68fcb25cac88e..741ad12efe967 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp @@ -17,14 +17,14 @@ #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; struct PullOutPath { std::vector partial_paths{}; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index 9a2749759d03e..e49440894f901 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -21,16 +21,16 @@ #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/util.hpp" -#include #include +#include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; +using tier4_planning_msgs::msg::PathWithLaneId; enum class PlannerType { NONE = 0, diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index f831eeda9778d..3bded7ee99fbb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 591d8adb60819..a89b1e5e31c56 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -31,7 +31,7 @@ #include #include -#include +#include #include #include @@ -306,7 +306,8 @@ class StartPlannerModule : public SceneModuleInterface const double velocity_threshold, const double object_check_backward_distance, const double object_check_forward_distance) const; bool needToPrepareBlinkerBeforeStartDrivingForward() const; - bool hasFinishedPullOut() const; + bool hasReachedFreespaceEnd() const; + bool hasReachedPullOutEnd() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; bool isStopped(); diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp index 5d55ce1519217..bac772627a849 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp @@ -23,11 +23,11 @@ #include -#include -#include -#include +#include +#include #include #include +#include #include @@ -36,13 +36,13 @@ namespace behavior_path_planner::start_planner_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; +using tier4_planning_msgs::msg::PathWithLaneId; PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 794577d7cc7aa..d5731f54224b6 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -101,6 +101,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(ns + "arc_path_interval"); p.parallel_parking_parameters.pull_out_lane_departure_margin = node->declare_parameter(ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + node->declare_parameter(ns + "lane_departure_check_expansion_margin"); p.parallel_parking_parameters.pull_out_max_steer_angle = node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg p.parallel_parking_parameters.center_line_path_interval = @@ -435,6 +437,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "lane_departure_margin", p->parallel_parking_parameters.pull_out_lane_departure_margin); + updateParam( + parameters, ns + "lane_departure_check_expansion_margin", + p->lane_departure_check_expansion_margin); updateParam( parameters, ns + "pull_out_max_steer_angle", p->parallel_parking_parameters.pull_out_max_steer_angle); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 280e1112ef7fb..7e1f38f30c378 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -65,6 +65,11 @@ StartPlannerModule::StartPlannerModule( { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); + lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters->lane_departure_check_expansion_margin; + + lane_departure_checker_->setParam(lane_departure_checker_params); // set enabled planner if (parameters_->enable_shift_pull_out) { @@ -579,7 +584,35 @@ bool StartPlannerModule::isExecutionReady() const bool StartPlannerModule::canTransitSuccessState() { - return hasFinishedPullOut(); + // Freespace Planner: + // - Can transit to success if the goal position is reached. + // - Cannot transit to success if the goal position is not reached. + if (status_.planner_type == PlannerType::FREESPACE) { + if (hasReachedFreespaceEnd()) { + RCLCPP_DEBUG( + getLogger(), "Transit to success: Freespace planner reached the end point of the path."); + return true; + } + return false; + } + + // Other Planners: + // - Cannot transit to success if the vehicle is driving in reverse. + // - Cannot transit to success if a safe path cannot be found due to: + // - Insufficient margin against static objects. + // - No path found that stays within the lane. + // In such cases, a stop point needs to be embedded and keep running start_planner. + // - Can transit to success if the end point of the pullout path is reached. + if (!status_.driving_forward || !status_.found_pull_out_path) { + return false; + } + + if (hasReachedPullOutEnd()) { + RCLCPP_DEBUG(getLogger(), "Transit to success: Reached the end point of the pullout path."); + return true; + } + + return false; } BehaviorModuleOutput StartPlannerModule::plan() @@ -1180,17 +1213,16 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( return stop_objects_in_pull_out_lanes; } -bool StartPlannerModule::hasFinishedPullOut() const +bool StartPlannerModule::hasReachedFreespaceEnd() const { - if (!status_.driving_forward || !status_.found_pull_out_path) { - return false; - } + const auto & current_pose = planner_data_->self_odometry->pose.pose; + return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + parameters_->th_arrived_distance; +} +bool StartPlannerModule::hasReachedPullOutEnd() const +{ const auto current_pose = planner_data_->self_odometry->pose.pose; - if (status_.planner_type == PlannerType::FREESPACE) { - return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < - parameters_->th_arrived_distance; - } // check that ego has passed pull out end point const double backward_path_length = @@ -1205,9 +1237,8 @@ bool StartPlannerModule::hasFinishedPullOut() const // offset to not finish the module before engage constexpr double offset = 0.1; - const bool has_finished = arclength_current.length - arclength_pull_out_end.length > offset; - return has_finished; + return arclength_current.length - arclength_pull_out_end.length > offset; } bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const diff --git a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt index 682da3ae58d7e..7ff2419eed919 100644 --- a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt @@ -3,12 +3,13 @@ project(behavior_velocity_blind_spot_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp src/manager.cpp src/scene.cpp + src/decisions.cpp ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index 3c22d1fe65db5..d7131d8c19245 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -3,10 +3,12 @@ blind_spot: use_pass_judge_line: true stop_line_margin: 1.0 # [m] - backward_length: 50.0 # [m] + backward_detection_length: 100.0 # [m] ignore_width_from_center_line: 0.7 # [m] - max_future_movement_time: 10.0 # [second] - threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] opposite_adjacent_extend_width: 1.5 # [m] + max_future_movement_time: 10.0 # [second] + ttc_min: -5.0 # [s] + ttc_max: 5.0 # [s] + ttc_ego_minimal_velocity: 5.0 # [m/s] enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index 1148379c26f42..ebf760405a256 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index 5cc90afb0043d..a30836ab9f6a9 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -71,55 +71,16 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( return msg; } -visualization_msgs::msg::MarkerArray createPoseMarkerArray( - const geometry_msgs::msg::Pose & pose, const StateMachine::State & state, const std::string & ns, - const int64_t id, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - if (state == StateMachine::State::STOP) { - visualization_msgs::msg::Marker marker_line{}; - marker_line.header.frame_id = "map"; - marker_line.ns = ns + "_line"; - marker_line.id = id; - marker_line.lifetime = rclcpp::Duration::from_seconds(0.3); - marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker_line.action = visualization_msgs::msg::Marker::ADD; - marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker_line.scale = createMarkerScale(0.1, 0.0, 0.0); - marker_line.color = createMarkerColor(r, g, b, 0.999); - - const double yaw = tf2::getYaw(pose.orientation); - - const double a = 3.0; - geometry_msgs::msg::Point p0; - p0.x = pose.position.x - a * std::sin(yaw); - p0.y = pose.position.y + a * std::cos(yaw); - p0.z = pose.position.z; - marker_line.points.push_back(p0); - - geometry_msgs::msg::Point p1; - p1.x = pose.position.x + a * std::sin(yaw); - p1.y = pose.position.y - a * std::cos(yaw); - p1.z = pose.position.z; - marker_line.points.push_back(p1); - - msg.markers.push_back(marker_line); - } - - return msg; -} - } // namespace motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls() { motion_utils::VirtualWalls virtual_walls; - if (!isActivated() && !is_over_pass_judge_line_) { + if (debug_data_.virtual_wall_pose) { motion_utils::VirtualWall wall; wall.text = "blind_spot"; - wall.pose = debug_data_.virtual_wall_pose; + wall.pose = debug_data_.virtual_wall_pose.value(); wall.ns = std::to_string(module_id_) + "_"; virtual_walls.push_back(wall); } @@ -130,28 +91,14 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; - const auto state = state_machine_.getState(); const auto now = this->clock_->now(); - appendMarkerArray( - createPoseMarkerArray( - debug_data_.stop_point_pose, state, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), - &debug_marker_array, now); - - appendMarkerArray( - createPoseMarkerArray( - debug_data_.judge_point_pose, state, "judge_point_pose", module_id_, 1.0, 1.0, 0.5), - &debug_marker_array, now); - - appendMarkerArray( - createLaneletPolygonsMarkerArray( - debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5), - &debug_marker_array, now); - - appendMarkerArray( - createLaneletPolygonsMarkerArray( - debug_data_.detection_areas, "detection_area", module_id_, 0.5, 0.0, 0.0), - &debug_marker_array, now); + if (debug_data_.detection_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.detection_area.value()}, "detection_area", module_id_, 0.5, 0.0, 0.0), + &debug_marker_array, now); + } appendMarkerArray( debug::createObjectsMarkerArray( diff --git a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp new file mode 100644 index 0000000000000..6e4135c50b253 --- /dev/null +++ b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp @@ -0,0 +1,153 @@ +// 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 "scene.hpp" + +#include + +namespace behavior_velocity_planner +{ + +/* + * for default + */ +template +void BlindSpotModule::setRTCStatusByDecision( + const T &, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + static_assert("Unsupported type passed to setRTCStatus"); + return; +} + +template +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const T & decision, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + static_assert("Unsupported type passed to reactRTCApprovalByDecision"); +} + +/* + * for InternalError + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + [[maybe_unused]] const InternalError & decision, + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const InternalError & decision, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + return; +} + +/* + * For OverPassJudge + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + [[maybe_unused]] const OverPassJudge & decision, + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const OverPassJudge & decision, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + return; +} + +/* + * for Unsafe + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + const Unsafe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + setSafe(false); + const auto & current_pose = planner_data_->current_odometry->pose; + setDistance( + motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason) +{ + if (!isActivated()) { + constexpr double stop_vel = 0.0; + planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose( + decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + } + return; +} + +/* + * for Safe + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + const Safe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + setSafe(true); + const auto & current_pose = planner_data_->current_odometry->pose; + setDistance( + motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) +{ + if (!isActivated()) { + constexpr double stop_vel = 0.0; + planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose( + decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + } + return; +} + +} // namespace 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 faa10c1fbe9b4..f1b0a56018d9b 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -36,21 +36,23 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) planner_param_.use_pass_judge_line = getOrDeclareParameter(node, ns + ".use_pass_judge_line"); planner_param_.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); - planner_param_.backward_length = getOrDeclareParameter(node, ns + ".backward_length"); + planner_param_.backward_detection_length = + getOrDeclareParameter(node, ns + ".backward_detection_length"); planner_param_.ignore_width_from_center_line = getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); - planner_param_.max_future_movement_time = - getOrDeclareParameter(node, ns + ".max_future_movement_time"); - planner_param_.threshold_yaw_diff = - getOrDeclareParameter(node, ns + ".threshold_yaw_diff"); planner_param_.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); planner_param_.opposite_adjacent_extend_width = getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); + planner_param_.max_future_movement_time = + getOrDeclareParameter(node, ns + ".max_future_movement_time"); + planner_param_.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); + planner_param_.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); + planner_param_.ttc_ego_minimal_velocity = + getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); } -void BlindSpotModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -63,14 +65,17 @@ void BlindSpotModuleManager::launchNewModules( } // Is turning lane? - const std::string turn_direction = ll.attributeOr("turn_direction", "else"); - if (turn_direction != "left" && turn_direction != "right") { + const std::string turn_direction_str = ll.attributeOr("turn_direction", "else"); + if (turn_direction_str != "left" && turn_direction_str != "right") { continue; } + const auto turn_direction = turn_direction_str == "left" + ? BlindSpotModule::TurnDirection::LEFT + : BlindSpotModule::TurnDirection::RIGHT; registerModule(std::make_shared( - module_id, lane_id, planner_data_, planner_param_, logger_.get_child("blind_spot_module"), - clock_)); + module_id, lane_id, turn_direction, planner_data_, planner_param_, + logger_.get_child("blind_spot_module"), clock_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), @@ -80,7 +85,7 @@ void BlindSpotModuleManager::launchNewModules( std::function &)> BlindSpotModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.hpp b/planning/behavior_velocity_blind_spot_module/src/manager.hpp index 9aeaa0abfc4b7..7d6f936cc9d5b 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC private: BlindSpotModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class BlindSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 7a3ea3160cb24..14ebe70216d5a 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -44,170 +44,148 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -namespace -{ -[[maybe_unused]] geometry_msgs::msg::Polygon toGeomPoly(const lanelet::CompoundPolygon3d & poly) -{ - geometry_msgs::msg::Polygon geom_poly; - - for (const auto & p : poly) { - geometry_msgs::msg::Point32 geom_point; - geom_point.x = p.x(); - geom_point.y = p.y(); - geom_point.z = p.z(); - geom_poly.points.push_back(geom_point); - } - - return geom_poly; -} -} // namespace - BlindSpotModule::BlindSpotModule( - const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) + const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, + const std::shared_ptr planner_data, const PlannerParam & planner_param, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), planner_param_{planner_param}, - turn_direction_(TurnDirection::INVALID), + turn_direction_(turn_direction), is_over_pass_judge_line_(false) { velocity_factor_.init(PlanningBehavior::REAR_CHECK); - - const auto & assigned_lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); - const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - if (!turn_direction.compare("left")) { - turn_direction_ = TurnDirection::LEFT; - } else if (!turn_direction.compare("right")) { - turn_direction_ = TurnDirection::RIGHT; - } + sibling_straight_lanelet_ = getSiblingStraightLanelet(planner_data); } -bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +void BlindSpotModule::initializeRTCStatus() { - debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); - - const auto input_path = *path; - - const auto current_state = state_machine_.getState(); - RCLCPP_DEBUG( - logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); - - /* get current pose */ - const auto & current_pose = planner_data_->current_odometry->pose; + setSafe(true); + setDistance(std::numeric_limits::lowest()); +} - /* get lanelet map */ - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); +BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( + PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + if (planner_param_.use_pass_judge_line && is_over_pass_judge_line_) { + return OverPassJudge{"already over the pass judge line. no plan needed."}; + } + const auto & input_path = *path; /* set stop-line and stop-judgement-line for base_link */ const auto interpolated_path_info_opt = generateInterpolatedPathInfo(input_path); if (!interpolated_path_info_opt) { - RCLCPP_DEBUG(logger_, "[BlindSpotModule::run] failed to interpolate path"); - return false; + return InternalError{"Failed to interpolate path"}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); - if (!sibling_straight_lanelet_) { - sibling_straight_lanelet_ = getSiblingStraightLanelet(); - } - const auto stoplines_idx_opt = generateStopLine(interpolated_path_info, path); if (!stoplines_idx_opt) { - RCLCPP_DEBUG(logger_, "[BlindSpotModule::run] setStopLineIdx fail"); - return false; + return InternalError{"generateStopLine fail"}; } const auto [default_stopline_idx, critical_stopline_idx] = stoplines_idx_opt.value(); - - if (default_stopline_idx <= 0) { - RCLCPP_DEBUG( - logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning."); - *path = input_path; // reset path - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return true; + if (default_stopline_idx == 0) { + return InternalError{"stop line or pass judge line is at path[0], ignore planning."}; } /* calc closest index */ + const auto & current_pose = planner_data_->current_odometry->pose; const auto closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); const auto stop_line_idx = closest_idx > default_stopline_idx ? critical_stopline_idx : default_stopline_idx; - - /* set judge line dist */ - const double current_vel = planner_data_->current_velocity->twist.linear.x; - const double max_acc = planner_data_->max_stop_acceleration_threshold; - const double delay_response_time = planner_data_->delay_response_time; - const double pass_judge_line_dist = - planning_utils::calcJudgeLineDistWithAccLimit(current_vel, max_acc, delay_response_time); - const auto stop_point_pose = path->points.at(stop_line_idx).point.pose; - const auto ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); - const size_t stop_point_segment_idx = - motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); - const auto distance_until_stop = motion_utils::calcSignedArcLength( - input_path.points, current_pose.position, ego_segment_idx, stop_point_pose.position, - stop_point_segment_idx); - - /* get debug info */ const auto stop_line_pose = planning_utils::getAheadPose( - stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); - debug_data_.virtual_wall_pose = stop_line_pose; - const auto stop_pose = path->points.at(stop_line_idx).point.pose; - debug_data_.stop_point_pose = stop_pose; + stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, input_path); - /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ - if (planner_param_.use_pass_judge_line) { - const double eps = 1e-1; // to prevent hunting - if ( - current_state == StateMachine::State::GO && - distance_until_stop + eps < pass_judge_line_dist) { - RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - *path = input_path; // reset path - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return true; // no plan needed. + const auto is_over_pass_judge = isOverPassJudge(input_path, stop_line_pose); + if (is_over_pass_judge) { + is_over_pass_judge_line_ = true; + return is_over_pass_judge.value(); + } + + if (!blind_spot_lanelets_) { + const auto blind_spot_lanelets = generateBlindSpotLanelets(input_path); + if (!blind_spot_lanelets.empty()) { + blind_spot_lanelets_ = blind_spot_lanelets; } } + if (!blind_spot_lanelets_) { + return InternalError{"There are not blind_spot_lanelets"}; + } + const auto & blind_spot_lanelets = blind_spot_lanelets_.value(); - /* get dynamic object */ - const auto objects_ptr = planner_data_->predicted_objects; + const auto detection_area_opt = generateBlindSpotPolygons( + input_path, closest_idx, blind_spot_lanelets, + path->points.at(critical_stopline_idx).point.pose); + if (!detection_area_opt) { + return InternalError{"Failed to generate BlindSpotPolygons"}; + } + const auto & detection_area = detection_area_opt.value(); + debug_data_.detection_area = detection_area; + const auto ego_time_to_reach_stop_line = computeTimeToPassStopLine( + blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose); /* calculate dynamic collision around detection area */ - bool has_obstacle = checkObstacleInBlindSpot( - lanelet_map_ptr, routing_graph_ptr, *path, objects_ptr, closest_idx, stop_line_pose); + const auto collision_obstacle = isCollisionDetected( + blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose, detection_area, + ego_time_to_reach_stop_line); state_machine_.setStateWithMarginTime( - has_obstacle ? StateMachine::State::STOP : StateMachine::State::GO, + collision_obstacle.has_value() ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("state_machine"), *clock_); - /* set stop speed */ - setSafe(state_machine_.getState() != StateMachine::State::STOP); - setDistance(motion_utils::calcSignedArcLength( - path->points, current_pose.position, path->points.at(stop_line_idx).point.pose.position)); - if (!isActivated()) { - constexpr double stop_vel = 0.0; - planning_utils::setVelocityFromIndex(stop_line_idx, stop_vel, path); - - /* get stop point and stop factor */ - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.stop_point_pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); - planning_utils::appendStopReason(stop_factor, stop_reason); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); - } else { - *path = input_path; // reset path + if (state_machine_.getState() == StateMachine::State::STOP) { + return Unsafe{stop_line_idx, collision_obstacle}; } + + return Safe{stop_line_idx}; +} + +// template-specification based visitor pattern +// https://en.cppreference.com/w/cpp/utility/variant/visit +template +struct VisitorSwitch : Ts... +{ + using Ts::operator()...; +}; +template +VisitorSwitch(Ts...) -> VisitorSwitch; + +void BlindSpotModule::setRTCStatus( + const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + std::visit( + VisitorSwitch{[&](const auto & sub_decision) { setRTCStatusByDecision(sub_decision, path); }}, + decision); +} + +void BlindSpotModule::reactRTCApproval( + const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason) +{ + std::visit( + VisitorSwitch{[&](const auto & sub_decision) { + reactRTCApprovalByDecision(sub_decision, path, stop_reason); + }}, + decision); +} + +bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + debug_data_ = DebugData(); + *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); + + initializeRTCStatus(); + const auto decision = modifyPathVelocityDetail(path, stop_reason); + const auto & input_path = *path; + setRTCStatus(decision, input_path); + reactRTCApproval(decision, path, stop_reason); + return true; } static bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) { for (const auto & pid : p.lane_ids) { if (pid == id) { @@ -218,7 +196,7 @@ static bool hasLaneIds( } static std::optional> findLaneIdInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) + const tier4_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) { bool found = false; size_t start = 0; @@ -245,7 +223,7 @@ static std::optional> findLaneIdInterval( } std::optional BlindSpotModule::generateInterpolatedPathInfo( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path) const + const tier4_planning_msgs::msg::PathWithLaneId & input_path) const { constexpr double ds = 0.2; InterpolatedPathInfo interpolated_path_info; @@ -259,10 +237,11 @@ std::optional BlindSpotModule::generateInterpolatedPathInf return interpolated_path_info; } -std::optional BlindSpotModule::getSiblingStraightLanelet() const +std::optional BlindSpotModule::getSiblingStraightLanelet( + const std::shared_ptr planner_data) const { - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map_ptr = planner_data->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data->route_handler_->getRoutingGraphPtr(); const auto assigned_lane = lanelet_map_ptr->laneletLayer.get(lane_id_); for (const auto & prev : routing_graph_ptr->previous(assigned_lane)) { @@ -297,8 +276,7 @@ static std::optional getFirstPointIntersectsLineByFootprint( } static std::optional getDuplicatedPointIdx( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & point) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) { for (size_t i = 0; i < path.points.size(); i++) { const auto & p = path.points.at(i).point.pose.position; @@ -313,8 +291,7 @@ static std::optional getDuplicatedPointIdx( } static std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); @@ -326,9 +303,8 @@ static std::optional insertPointIndex( inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) - int insert_idx = closest_idx; - autoware_auto_planning_msgs::msg::PathPointWithLaneId inserted_point = - inout_path->points.at(closest_idx); + size_t insert_idx = closest_idx; + tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { ++insert_idx; } else { @@ -347,8 +323,9 @@ static std::optional insertPointIndex( std::optional> BlindSpotModule::generateStopLine( const InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * path) const + tier4_planning_msgs::msg::PathWithLaneId * path) const { + // NOTE: this is optionally int for later subtraction const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interpolated_path_info.ds); @@ -368,7 +345,9 @@ std::optional> BlindSpotModule::generateStopLine( if (!first_conflict_idx_ip_opt) { return std::nullopt; } - const int first_conflict_idx_ip = static_cast(first_conflict_idx_ip_opt.value()); + + // NOTE: this is optionally int for later subtraction + const auto first_conflict_idx_ip = static_cast(first_conflict_idx_ip_opt.value()); stop_idx_default_ip = static_cast(std::max(first_conflict_idx_ip - margin_idx_dist, 0)); stop_idx_critical_ip = static_cast(first_conflict_idx_ip); @@ -406,128 +385,113 @@ std::optional> BlindSpotModule::generateStopLine( return std::make_pair(stopline_idx_default_opt.value(), stopline_idx_critical_opt.value()); } -void BlindSpotModule::cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, const double time_thr) const +autoware_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictPathWithDuration( + const std_msgs::msg::Header & header, + const autoware_perception_msgs::msg::PredictedObject & object_original, + const double time_thr) const { + auto object = object_original; const rclcpp::Time current_time = clock_->now(); - for (auto & object : objects_ptr->objects) { // each objects - for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths - const auto origin_path = predicted_path; - predicted_path.path.clear(); - - for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points - const auto & predicted_pose = origin_path.path.at(k); - const auto predicted_time = - rclcpp::Time(objects_ptr->header.stamp) + - rclcpp::Duration(origin_path.time_step) * static_cast(k); - if ((predicted_time - current_time).seconds() < time_thr) { - predicted_path.path.push_back(predicted_pose); - } + for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths + const auto origin_path = predicted_path; + predicted_path.path.clear(); + + for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points + const auto & predicted_pose = origin_path.path.at(k); + const auto predicted_time = rclcpp::Time(header.stamp) + + rclcpp::Duration(origin_path.time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + predicted_path.path.push_back(predicted_pose); } } } + return object; } -bool BlindSpotModule::checkObstacleInBlindSpot( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) +std::optional BlindSpotModule::isOverPassJudge( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const geometry_msgs::msg::Pose & stop_point_pose) const { - /* get detection area */ - if (turn_direction_ == TurnDirection::INVALID) { - RCLCPP_WARN(logger_, "blind spot detector is running, turn_direction_ = not right or left."); - return false; - } - - const auto areas_opt = generateBlindSpotPolygons( - lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose); - if (areas_opt) { - const auto & detection_areas = areas_opt.value().detection_areas; - const auto & conflict_areas = areas_opt.value().conflict_areas; - const auto & opposite_detection_areas = areas_opt.value().opposite_detection_areas; - const auto & opposite_conflict_areas = areas_opt.value().opposite_conflict_areas; - debug_data_.detection_areas = detection_areas; - debug_data_.conflict_areas = conflict_areas; - debug_data_.detection_areas.insert( - debug_data_.detection_areas.end(), opposite_detection_areas.begin(), - opposite_detection_areas.end()); - debug_data_.conflict_areas.insert( - debug_data_.conflict_areas.end(), opposite_conflict_areas.begin(), - opposite_conflict_areas.end()); - - autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr; - cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); - - // check objects in blind spot areas - for (const auto & object : objects.objects) { - if (!isTargetObjectType(object)) { - continue; - } + const auto & current_pose = planner_data_->current_odometry->pose; - // right direction - const bool exist_in_right_detection_area = - std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { - return bg::within( - to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), - lanelet::utils::to2D(area)); - }); - // opposite direction - const bool exist_in_opposite_detection_area = std::any_of( - opposite_detection_areas.begin(), opposite_detection_areas.end(), - [&object](const auto & area) { - return bg::within( - to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), - lanelet::utils::to2D(area)); - }); - const bool exist_in_detection_area = - exist_in_right_detection_area || exist_in_opposite_detection_area; - if (!exist_in_detection_area) { - continue; - } - const bool exist_in_right_conflict_area = - isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose); - const bool exist_in_opposite_conflict_area = isPredictedPathInArea( - object, opposite_conflict_areas, planner_data_->current_odometry->pose); - const bool exist_in_conflict_area = - exist_in_right_conflict_area || exist_in_opposite_conflict_area; - if (exist_in_detection_area && exist_in_conflict_area) { - debug_data_.conflicting_targets.objects.push_back(object); - setObjectsOfInterestData( - object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); - return true; - } + /* set judge line dist */ + const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithAccLimit( + planner_data_->current_velocity->twist.linear.x, planner_data_->max_stop_acceleration_threshold, + planner_data_->delay_response_time); + const auto ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + const size_t stop_point_segment_idx = + motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); + const auto distance_until_stop = motion_utils::calcSignedArcLength( + input_path.points, current_pose.position, ego_segment_idx, stop_point_pose.position, + stop_point_segment_idx); + + /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ + if (planner_param_.use_pass_judge_line) { + const double eps = 1e-1; // to prevent hunting + if (const auto current_state = state_machine_.getState(); + current_state == StateMachine::State::GO && + distance_until_stop + eps < pass_judge_line_dist) { + return OverPassJudge{"over the pass judge line. no plan needed."}; } - return false; - } else { - return false; } + return std::nullopt; +} + +double BlindSpotModule::computeTimeToPassStopLine( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose) const +{ + // egoが停止している時にそのまま速度を使うと衝突しなくなってしまうのでegoについては最低速度を使う + const auto & current_pose = planner_data_->current_odometry->pose; + const auto current_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, current_pose).length; + const auto stopline_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length; + const auto remaining_distance = stopline_arc_ego - current_arc_ego; + return remaining_distance / std::max( + planner_param_.ttc_ego_minimal_velocity, + planner_data_->current_velocity->twist.linear.x); } -bool BlindSpotModule::isPredictedPathInArea( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const std::vector & areas, geometry_msgs::msg::Pose ego_pose) const +std::optional BlindSpotModule::isCollisionDetected( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, + const double ego_time_to_reach_stop_line) { - const auto ego_yaw = tf2::getYaw(ego_pose.orientation); - const auto threshold_yaw_diff = planner_param_.threshold_yaw_diff; - // NOTE: iterating all paths including those of low confidence - return std::any_of( - areas.begin(), areas.end(), [&object, &ego_yaw, &threshold_yaw_diff](const auto & area) { - const auto area_2d = lanelet::utils::to2D(area); - return std::any_of( - object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(), - [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & path) { - return std::any_of( - path.path.begin(), path.path.end(), - [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & point) { - const auto is_in_area = bg::within(to_bg2d(point.position), area_2d); - const auto match_yaw = - std::fabs(ego_yaw - tf2::getYaw(point.orientation)) < threshold_yaw_diff; - return is_in_area && match_yaw; - }); - }); - }); + // check objects in blind spot areas + const auto stop_line_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length; + for (const auto & original_object : planner_data_->predicted_objects->objects) { + if (!isTargetObjectType(original_object)) { + continue; + } + const auto object = cutPredictPathWithDuration( + planner_data_->predicted_objects->header, original_object, + planner_param_.max_future_movement_time); + // right direction + const bool exist_in_detection_area = bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); + if (!exist_in_detection_area) { + continue; + } + const auto object_arc_length = + lanelet::utils::getArcCoordinates( + blind_spot_lanelets, object.kinematics.initial_pose_with_covariance.pose) + .length; + const auto object_time_to_reach_stop_line = + (object_arc_length - stop_line_arc_ego) / + (object.kinematics.initial_twist_with_covariance.twist.linear.x); + const auto ttc = ego_time_to_reach_stop_line - object_time_to_reach_stop_line; + RCLCPP_INFO(logger_, "object ttc is %f", ttc); + if (planner_param_.ttc_min < ttc && ttc < planner_param_.ttc_max) { + return object; + } + } + return std::nullopt; } lanelet::ConstLanelet BlindSpotModule::generateHalfLanelet( @@ -554,8 +518,6 @@ lanelet::ConstLanelet BlindSpotModule::generateHalfLanelet( const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); auto half_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); - const auto centerline = lanelet::utils::generateFineCenterline(half_lanelet, 5.0); - half_lanelet.setCenterline(centerline); return half_lanelet; } @@ -620,15 +582,23 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet( return new_lanelet; } -std::optional BlindSpotModule::generateBlindSpotPolygons( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & stop_line_pose) const +static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) { + lanelet::Points3d pts; + for (const auto & pt : line) { + pts.push_back(lanelet::Point3d(pt)); + } + return lanelet::LineString3d(lanelet::InvalId, pts); +} + +lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( + const tier4_planning_msgs::msg::PathWithLaneId & path) const +{ + /* get lanelet map */ + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + std::vector lane_ids; - lanelet::ConstLanelets blind_spot_lanelets; - lanelet::ConstLanelets blind_spot_opposite_lanelets; /* get lane ids until intersection */ for (const auto & point : path.points) { bool found_intersection_lane = false; @@ -646,17 +616,10 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( if (found_intersection_lane) break; } - for (size_t i = 0; i < lane_ids.size(); ++i) { - const auto half_lanelet = - generateHalfLanelet(lanelet_map_ptr->laneletLayer.get(lane_ids.at(i))); - blind_spot_lanelets.push_back(half_lanelet); - } - - // additional detection area on left/right side - lanelet::ConstLanelets adjacent_lanelets; - lanelet::ConstLanelets opposite_adjacent_lanelets; + lanelet::ConstLanelets blind_spot_lanelets; for (const auto i : lane_ids) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); + const auto ego_half_lanelet = generateHalfLanelet(lane); const auto adj = turn_direction_ == TurnDirection::LEFT ? (routing_graph_ptr->adjacentLeft(lane)) @@ -688,78 +651,54 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( return std::nullopt; } }(); - if (adj) { - const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); - adjacent_lanelets.push_back(half_lanelet); - } - if (opposite_adj) { - const auto half_lanelet = + + if (!adj && !opposite_adj) { + blind_spot_lanelets.push_back(ego_half_lanelet); + } else if (!!adj) { + const auto adj_half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); + const auto lefts = (turn_direction_ == TurnDirection::LEFT) ? adj_half_lanelet.leftBound() + : ego_half_lanelet.leftBound(); + const auto rights = (turn_direction_ == TurnDirection::RIGHT) ? adj_half_lanelet.rightBound() + : ego_half_lanelet.rightBound(); + blind_spot_lanelets.push_back( + lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); + } else if (opposite_adj) { + const auto adj_half_lanelet = generateExtendedOppositeAdjacentLanelet(opposite_adj.value(), turn_direction_); - opposite_adjacent_lanelets.push_back(half_lanelet); + const auto lefts = (turn_direction_ == TurnDirection::LEFT) ? adj_half_lanelet.leftBound() + : ego_half_lanelet.leftBound(); + const auto rights = (turn_direction_ == TurnDirection::LEFT) ? ego_half_lanelet.rightBound() + : adj_half_lanelet.rightBound(); + blind_spot_lanelets.push_back( + lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); } } + return blind_spot_lanelets; +} - const auto current_arc_ego = - lanelet::utils::getArcCoordinates(blind_spot_lanelets, path.points[closest_idx].point.pose) - .length; +std::optional BlindSpotModule::generateBlindSpotPolygons( + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose) const +{ const auto stop_line_arc_ego = lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length; - const auto detection_area_start_length_ego = stop_line_arc_ego - planner_param_.backward_length; - if (detection_area_start_length_ego < current_arc_ego && current_arc_ego < stop_line_arc_ego) { - BlindSpotPolygons blind_spot_polygons; - auto conflict_area_ego = lanelet::utils::getPolygonFromArcLength( - blind_spot_lanelets, current_arc_ego, stop_line_arc_ego); - auto detection_area_ego = lanelet::utils::getPolygonFromArcLength( - blind_spot_lanelets, detection_area_start_length_ego, stop_line_arc_ego); - blind_spot_polygons.conflict_areas.emplace_back(std::move(conflict_area_ego)); - blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_ego)); - // additional detection area on left/right side - if (!adjacent_lanelets.empty()) { - const auto stop_line_arc_adj = lanelet::utils::getLaneletLength3d(adjacent_lanelets); - const auto current_arc_adj = stop_line_arc_adj - (stop_line_arc_ego - current_arc_ego); - const auto detection_area_start_length_adj = - stop_line_arc_adj - planner_param_.backward_length; - auto conflicting_area_adj = lanelet::utils::getPolygonFromArcLength( - adjacent_lanelets, current_arc_adj, stop_line_arc_adj); - auto detection_area_adj = lanelet::utils::getPolygonFromArcLength( - adjacent_lanelets, detection_area_start_length_adj, stop_line_arc_adj); - blind_spot_polygons.conflict_areas.emplace_back(std::move(conflicting_area_adj)); - blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_adj)); - } - // additional detection area on left/right opposite lane side - if (!opposite_adjacent_lanelets.empty()) { - const auto stop_line_arc_opposite_adj = - lanelet::utils::getLaneletLength3d(opposite_adjacent_lanelets); - const auto current_arc_opposite_adj = - stop_line_arc_opposite_adj - (stop_line_arc_ego - current_arc_ego); - const auto detection_area_start_length_opposite_adj = - stop_line_arc_opposite_adj - planner_param_.backward_length; - auto conflicting_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( - opposite_adjacent_lanelets, current_arc_opposite_adj, stop_line_arc_opposite_adj); - auto detection_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( - opposite_adjacent_lanelets, detection_area_start_length_opposite_adj, - stop_line_arc_opposite_adj); - blind_spot_polygons.opposite_conflict_areas.emplace_back( - std::move(conflicting_area_opposite_adj)); - blind_spot_polygons.opposite_detection_areas.emplace_back( - std::move(detection_area_opposite_adj)); - } - return blind_spot_polygons; - } else { - return std::nullopt; - } + const auto detection_area_start_length_ego = + std::max(stop_line_arc_ego - planner_param_.backward_detection_length, 0.0); + return lanelet::utils::getPolygonFromArcLength( + blind_spot_lanelets, detection_area_start_length_ego, stop_line_arc_ego); } bool BlindSpotModule::isTargetObjectType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { if ( object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE || + autoware_perception_msgs::msg::ObjectClassification::BICYCLE || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { return true; } return false; diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index fd93661b33d6b..b3bf90f5b91d1 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -20,10 +20,10 @@ #include #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -31,25 +31,18 @@ #include #include #include +#include #include namespace behavior_velocity_planner { -struct BlindSpotPolygons -{ - std::vector conflict_areas; - std::vector detection_areas; - std::vector opposite_conflict_areas; - std::vector opposite_detection_areas; -}; - /** * @brief wrapper class of interpolated path with lane id */ struct InterpolatedPathInfo { /** the interpolated path */ - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; /** discretization interval of interpolation */ double ds{0.0}; /** the intersection lanelet id */ @@ -58,42 +51,63 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; +/** + * @brief represent action + */ +struct InternalError +{ + const std::string error; +}; + +struct OverPassJudge +{ + const std::string report; +}; + +struct Unsafe +{ + const size_t stop_line_idx; + const std::optional collision_obstacle; +}; + +struct Safe +{ + const size_t stop_line_idx; +}; + +using BlindSpotDecision = std::variant; + class BlindSpotModule : public SceneModuleInterface { public: - enum class TurnDirection { LEFT = 0, RIGHT, INVALID }; + enum class TurnDirection { LEFT, RIGHT }; struct DebugData { - geometry_msgs::msg::Pose virtual_wall_pose; - geometry_msgs::msg::Pose stop_point_pose; - geometry_msgs::msg::Pose judge_point_pose; - std::vector conflict_areas; - std::vector detection_areas; - std::vector opposite_conflict_areas; - std::vector opposite_detection_areas; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + std::optional virtual_wall_pose{std::nullopt}; + std::optional detection_area; + autoware_perception_msgs::msg::PredictedObjects conflicting_targets; }; public: struct PlannerParam { - bool use_pass_judge_line; //! distance which ego can stop with max brake - double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary - double backward_length; //! distance[m] from closest path point to the edge of beginning point - double ignore_width_from_center_line; //! ignore width from center line from detection_area - double - max_future_movement_time; //! maximum time[second] for considering future movement of object - double threshold_yaw_diff; //! threshold of yaw difference between ego and target object - double - adjacent_extend_width; //! the width of extended detection/conflict area on adjacent lane + bool use_pass_judge_line; + double stop_line_margin; + double backward_detection_length; + double ignore_width_from_center_line; + double adjacent_extend_width; double opposite_adjacent_extend_width; + double max_future_movement_time; + double ttc_min; + double ttc_max; + double ttc_ego_minimal_velocity; }; BlindSpotModule( - const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, + const std::shared_ptr planner_data, const PlannerParam & planner_param, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path @@ -105,18 +119,39 @@ class BlindSpotModule : public SceneModuleInterface std::vector createVirtualWalls() override; private: + // (semi) const variables const int64_t lane_id_; const PlannerParam planner_param_; - TurnDirection turn_direction_{TurnDirection::INVALID}; - bool is_over_pass_judge_line_{false}; + const TurnDirection turn_direction_; std::optional sibling_straight_lanelet_{std::nullopt}; + std::optional blind_spot_lanelets_{std::nullopt}; + + // state variables + bool is_over_pass_judge_line_{false}; // Parameter + void initializeRTCStatus(); + BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + // setSafe(), setDistance() + void setRTCStatus( + const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); + template + void setRTCStatusByDecision( + const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); + // stop/GO + void reactRTCApproval( + const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason); + template + void reactRTCApprovalByDecision( + const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason); + std::optional generateInterpolatedPathInfo( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path) const; + const tier4_planning_msgs::msg::PathWithLaneId & input_path) const; - std::optional getSiblingStraightLanelet() const; + std::optional getSiblingStraightLanelet( + const std::shared_ptr planner_data) const; /** * @brief Generate a stop line and insert it into the path. @@ -129,7 +164,15 @@ class BlindSpotModule : public SceneModuleInterface */ std::optional> generateStopLine( const InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * path) const; + tier4_planning_msgs::msg::PathWithLaneId * path) const; + + std::optional isOverPassJudge( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & stop_point_pose) const; + + double computeTimeToPassStopLine( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose) const; /** * @brief Check obstacle is in blind spot areas. @@ -141,12 +184,10 @@ class BlindSpotModule : public SceneModuleInterface * @param closest_idx closest path point index from ego car in path points * @return true when an object is detected in blind spot */ - bool checkObstacleInBlindSpot( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose); + std::optional isCollisionDetected( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, + const double ego_time_to_reach_stop_line); /** * @brief Create half lanelet @@ -160,6 +201,9 @@ class BlindSpotModule : public SceneModuleInterface lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; + lanelet::ConstLanelets generateBlindSpotLanelets( + const tier4_planning_msgs::msg::PathWithLaneId & path) const; + /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. * Broad area is made from backward expanded point to stop line point @@ -167,10 +211,9 @@ class BlindSpotModule : public SceneModuleInterface * @param closest_idx closest path point index from ego car in path points * @return Blind spot polygons */ - std::optional generateBlindSpotPolygons( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + std::optional generateBlindSpotPolygons( + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & pose) const; /** @@ -178,26 +221,16 @@ class BlindSpotModule : public SceneModuleInterface * @param object Dynamic object * @return True when object belong to targeted classes */ - bool isTargetObjectType(const autoware_auto_perception_msgs::msg::PredictedObject & object) const; - - /** - * @brief Check if at least one of object's predicted position is in area - * @param object Dynamic object - * @param area Area defined by polygon - * @return True when at least one of object's predicted position is in area - */ - bool isPredictedPathInArea( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const std::vector & areas, geometry_msgs::msg::Pose ego_pose) const; + bool isTargetObjectType(const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. * @param objects_ptr target objects * @param time_thr time threshold to cut path */ - void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const double time_thr) const; + autoware_perception_msgs::msg::PredictedObject cutPredictPathWithDuration( + const std_msgs::msg::Header & header, + const autoware_perception_msgs::msg::PredictedObject & object, const double time_thr) const; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt index 69dd1ced695ec..f21d5ab786cad 100644 --- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_crosswalk_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index b82ea7887dc98..23222d2841bad 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -221,21 +221,21 @@ To inflate the masking behind objects, their footprint can be made bigger using ![stuck_vehicle_attention_range](docs/with_occlusion.svg){width=600} -| Parameter | Unit | Type | Description | -| ---------------------------------------------- | ----- | ----------- | ----------------------------------------------------------------------------------------------------------------------------------------------- | -| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded | -| `occluded_object_velocity` | [m/s] | double | assumed velocity of objects that may come out of the occluded space | -| `slow_down_velocity` | [m/s] | double | slow down velocity | -| `time_buffer` | [s] | double | consecutive time with/without an occlusion to add/remove the slowdown | -| `min_size` | [m] | double | minimum size of an occlusion (square side size) | -| `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | -| `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | -| `ignore_with_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | -| `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | -| `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | -| `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_auto_perception_msgs::msg::ObjectClassification` for all the labels) | -| `ignore_velocity_thresholds.custom_thresholds` | [-] | double list | velocities of the custom labels | -| `extra_predicted_objects_size` | [m] | double | extra size added to the objects for masking the occlusions | +| Parameter | Unit | Type | Description | +| ---------------------------------------------- | ----- | ----------- | ------------------------------------------------------------------------------------------------------------------------------------------ | +| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded | +| `occluded_object_velocity` | [m/s] | double | assumed velocity of objects that may come out of the occluded space | +| `slow_down_velocity` | [m/s] | double | slow down velocity | +| `time_buffer` | [s] | double | consecutive time with/without an occlusion to add/remove the slowdown | +| `min_size` | [m] | double | minimum size of an occlusion (square side size) | +| `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | +| `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | +| `ignore_with_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | +| `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | +| `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | +| `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_perception_msgs::msg::ObjectClassification` for all the labels) | +| `ignore_velocity_thresholds.custom_thresholds` | [-] | double list | velocities of the custom labels | +| `extra_predicted_objects_size` | [m] | double | extra size added to the objects for masking the occlusions | ### Others diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 67b923a8d4cd8..e0d25c56b1610 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -83,6 +83,6 @@ ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored ignore_velocity_thresholds: default: 0.5 # [m/s] occlusions are only ignored behind objects with a higher or equal velocity - custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels) + custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_perception_msgs::msg::ObjectClassification for all the labels) custom_thresholds: [0.0] # velocities of the custom labels extra_predicted_objects_size: 0.5 # [m] extra size added to the objects for masking the occlusions 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 d287d7e705542..690eca196d536 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 @@ -35,14 +35,14 @@ #include #include -#include +#include #include namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; @@ -87,14 +87,12 @@ struct DebugData std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs); std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs); bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr); diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 2c38a9836e0af..2fd5e2bb514c3 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -21,10 +21,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 5110ff9993a62..fe4282893674c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -143,19 +143,17 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); cp.occlusion_ignore_velocity_thresholds.resize( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_thresholds.default")); const auto get_label = [](const std::string & s) { - if (s == "CAR") return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; - if (s == "TRUCK") return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; - if (s == "BUS") return autoware_auto_perception_msgs::msg::ObjectClassification::BUS; - if (s == "TRAILER") return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; - if (s == "MOTORCYCLE") - return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; - if (s == "BICYCLE") return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; - if (s == "PEDESTRIAN") - return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + if (s == "CAR") return autoware_perception_msgs::msg::ObjectClassification::CAR; + if (s == "TRUCK") return autoware_perception_msgs::msg::ObjectClassification::TRUCK; + if (s == "BUS") return autoware_perception_msgs::msg::ObjectClassification::BUS; + if (s == "TRAILER") return autoware_perception_msgs::msg::ObjectClassification::TRAILER; + if (s == "MOTORCYCLE") return autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE; + if (s == "BICYCLE") return autoware_perception_msgs::msg::ObjectClassification::BICYCLE; + if (s == "PEDESTRIAN") return autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; }; const auto custom_labels = getOrDeclareParameter>( node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels"); diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index 38bb63c121699..ba7e75193deab 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -23,8 +23,8 @@ #include #include -#include #include +#include #include #include @@ -35,7 +35,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC { diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 2455b36d5883f..b6adb32f642eb 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -84,11 +84,11 @@ std::vector calculate_detection_areas( return detection_areas; } -std::vector select_and_inflate_objects( - const std::vector & objects, +std::vector select_and_inflate_objects( + const std::vector & objects, const std::vector velocity_thresholds, const double inflate_size) { - std::vector selected_objects; + std::vector selected_objects; for (const auto & o : objects) { const auto vel_threshold = velocity_thresholds[o.classification.front().label]; if (o.kinematics.initial_twist_with_covariance.twist.linear.x >= vel_threshold) { @@ -103,7 +103,7 @@ std::vector select_and_infl void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, - const std::vector & objects) + const std::vector & objects) { const auto angle_cmp = [&](const auto & p1, const auto & p2) { const auto d1 = p1 - grid_map.getPosition(); @@ -139,7 +139,7 @@ bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, - const std::vector & dynamic_objects, + const std::vector & dynamic_objects, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::GridMap grid_map; diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index a76fdeb549b88..4aab9d3bfc888 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -61,7 +61,7 @@ bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, - const std::vector & dynamic_objects, + const std::vector & dynamic_objects, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief calculate the distance away from the crosswalk that should be checked for occlusions @@ -78,8 +78,8 @@ double calculate_detection_range( /// @param skip_pedestrians if true, pedestrians are not selected regardless of their velocities /// @param inflate_size [m] size by which the shape of the selected objects are inflated /// @return selected and inflated objects -std::vector select_and_inflate_objects( - const std::vector & objects, +std::vector select_and_inflate_objects( + const std::vector & objects, const double velocity_threshold, const bool skip_pedestrians, const double inflate_size); /// @brief clear occlusions behind the given objects @@ -88,7 +88,7 @@ std::vector select_and_infl /// @param objects objects void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, - const std::vector & objects); + const std::vector & objects); } // namespace 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 0f9c753523f13..191eea8feabed 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -16,7 +16,6 @@ #include "occluded_crosswalk.hpp" -#include #include #include #include @@ -675,7 +674,7 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( } std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( - const autoware_auto_perception_msgs::msg::PredictedPath & path) const + const autoware_perception_msgs::msg::PredictedPath & path) const { using tier4_autoware_utils::Segment2d; @@ -1128,8 +1127,7 @@ bool CrosswalkModule::isRedSignalForPedestrians() const } for (const auto & element : lights) { - if ( - element.color == TrafficSignalElement::RED && element.shape == TrafficSignalElement::CIRCLE) + if (element.color == TrafficLightElement::RED && element.shape == TrafficLightElement::CIRCLE) return true; } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index e9fdc38304603..107a904dd076b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -49,15 +49,15 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightElement; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; +using tier4_planning_msgs::msg::PathWithLaneId; namespace { @@ -182,7 +182,7 @@ class CrosswalkModule : public SceneModuleInterface const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel, const bool is_ego_yielding, const std::optional & collision_point, const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon, - const bool is_object_on_ego_path) + const bool is_object_away_from_path) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -202,7 +202,7 @@ class CrosswalkModule : public SceneModuleInterface planner_param.timeout_set_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; - if (is_ego_yielding && !intent_to_cross && !is_object_on_ego_path) { + if (is_ego_yielding && !intent_to_cross && is_object_away_from_path) { collision_state = CollisionState::IGNORE; return; } @@ -261,15 +261,16 @@ class CrosswalkModule : public SceneModuleInterface // update current uuids current_uuids_.push_back(uuid); - const bool is_object_on_ego_path = - boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) < - 0.5; + const bool is_object_away_from_path = + !attention_area.outer().empty() && + boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) > + 0.5; // add new object if (objects.count(uuid) == 0) { if ( has_traffic_light && planner_param.disable_yield_for_new_stopped_object && - !is_object_on_ego_path) { + is_object_away_from_path) { objects.emplace(uuid, ObjectInfo{CollisionState::IGNORE}); } else { objects.emplace(uuid, ObjectInfo{CollisionState::YIELD}); @@ -279,7 +280,7 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( now, position, vel, is_ego_yielding, collision_point, planner_param, crosswalk_polygon, - is_object_on_ego_path); + is_object_away_from_path); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; objects.at(uuid).classification = classification; @@ -352,7 +353,7 @@ class CrosswalkModule : public SceneModuleInterface std::optional findEgoPassageDirectionAlongPath( const PathWithLaneId & sparse_resample_path) const; std::optional findObjectPassageDirectionAlongVehicleLane( - const autoware_auto_perception_msgs::msg::PredictedPath & path) const; + const autoware_perception_msgs::msg::PredictedPath & path) const; std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index ee1b6347c9e73..d276ae95e06b6 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -55,8 +55,7 @@ using tier4_autoware_utils::Point2d; std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::vector> crosswalks; @@ -90,8 +89,7 @@ std::vector> getCrosswalksOnPath( std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::set crosswalk_id_set; diff --git a/planning/behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_detection_area_module/CMakeLists.txt index 2a16ed95ba655..e6990d2d28642 100644 --- a/planning/behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_detection_area_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_detection_area_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_detection_area_module/package.xml index 66b8a62e83a2c..6bcca43edc0cb 100644 --- a/planning/behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_detection_area_module/package.xml @@ -17,7 +17,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index 8e9a6b6a58a96..feb5bf6bb50ef 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -51,7 +51,7 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) } void DetectionAreaModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & detection_area_with_lane_id : planning_utils::getRegElemMapOnPath( @@ -74,7 +74,7 @@ void DetectionAreaModuleManager::launchNewModules( std::function &)> DetectionAreaModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_detection_area_module/src/manager.hpp index 10fca7182d09a..71cfa0a5eef96 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC private: DetectionAreaModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class DetectionAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_detection_area_module/src/scene.hpp index c3f3f81cd6dfd..f135c3b2660cb 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.hpp @@ -36,7 +36,7 @@ namespace behavior_velocity_planner using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class DetectionAreaModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt index 20f4b3aa8f4d3..481ac245cb432 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_dynamic_obstacle_stop_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml index 7e9599c49bc2d..84d01d04a09a4 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -12,8 +12,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs libboost-dev 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 d57789d574ad9..8497369917232 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -62,7 +62,7 @@ std::optional find_closest_collision_point( std::vector find_collisions( const EgoData & ego_data, - const std::vector & objects, + const std::vector & objects, const tier4_autoware_utils::MultiPolygon2d & object_forward_footprints) { std::vector collisions; 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 b65baaeb46aa3..27a48afa032b1 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -38,7 +38,7 @@ std::optional find_closest_collision_point( /// @return the point of earliest collision along the ego path std::vector find_collisions( const EgoData & ego_data, - const std::vector & objects, + const std::vector & objects, const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints); } // namespace behavior_velocity_planner::dynamic_obstacle_stop 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 b134ad3c39628..7397f63ca079c 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include 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 6a0c61963ac81..a58e4e9b88c75 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -29,7 +29,7 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop { tier4_autoware_utils::MultiPolygon2d make_forward_footprints( - const std::vector & obstacles, + const std::vector & obstacles, const PlannerParam & params, const double hysteresis) { tier4_autoware_utils::MultiPolygon2d forward_footprints; @@ -41,7 +41,7 @@ tier4_autoware_utils::MultiPolygon2d make_forward_footprints( } tier4_autoware_utils::Polygon2d make_forward_footprint( - const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis) { const auto & shape = obstacle.shape.dimensions; 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 c28e89ac6c9f6..0050a4e2c9259 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -30,7 +30,7 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop /// @param [in] hysteresis [m] extra lateral distance to add to the footprints /// @return forward footprint of the obstacle tier4_autoware_utils::MultiPolygon2d make_forward_footprints( - const std::vector & obstacles, + const std::vector & obstacles, const PlannerParam & params, const double hysteresis); /// @brief create the footprint of the given obstacle and its projection over a fixed time horizon /// @param [in] obstacle obstacle @@ -38,7 +38,7 @@ tier4_autoware_utils::MultiPolygon2d make_forward_footprints( /// @param [in] hysteresis [m] extra lateral distance to add to the footprint /// @return forward footprint of the obstacle tier4_autoware_utils::Polygon2d make_forward_footprint( - const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project 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 92004b4b9f249..63f1f3025f94a 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -50,7 +50,7 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node } void DynamicObstacleStopModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -62,7 +62,7 @@ void DynamicObstacleStopModuleManager::launchNewModules( std::function &)> DynamicObstacleStopModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; 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 e461cc9c16445..eb7bf4c6dbc98 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -43,10 +43,10 @@ class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class DynamicObstacleStopModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index c95db485781f9..205e110298f67 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -30,20 +30,19 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop /// @param params parameters /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects -std::vector filter_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, +std::vector filter_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { - std::vector filtered_objects; + std::vector filtered_objects; const auto is_vehicle = [](const auto & o) { return std::find_if(o.classification.begin(), o.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - c.label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + return c.label == autoware_perception_msgs::msg::ObjectClassification::CAR || + c.label == autoware_perception_msgs::msg::ObjectClassification::BUS || + c.label == autoware_perception_msgs::msg::ObjectClassification::TRUCK || + c.label == autoware_perception_msgs::msg::ObjectClassification::TRAILER || + c.label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + c.label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE; }) != o.classification.end(); }; const auto is_in_range = [&](const auto & o) { 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 22857f6db1bda..5daa0cda44203 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 @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include @@ -30,8 +30,8 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop /// @param params parameters /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects -std::vector filter_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, +std::vector filter_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); } // namespace behavior_velocity_planner::dynamic_obstacle_stop 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 55eaf079afee6..1df8a1ed6a4a3 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 @@ -23,7 +23,7 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop void update_object_map( ObjectStopDecisionMap & object_map, const std::vector & collisions, const rclcpp::Time & now, - const std::vector & path_points, + const std::vector & path_points, const PlannerParam & params) { for (auto & [object, decision] : object_map) decision.collision_detected = false; 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 cddff65da36d3..5756c8589fb73 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 @@ -61,7 +61,7 @@ using ObjectStopDecisionMap = std::unordered_map & collisions, const rclcpp::Time & now, - const std::vector & path_points, + const std::vector & path_points, const PlannerParam & params); /// @brief find the earliest collision requiring a stop along the ego path 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 21fca83565ee6..79911c40a195d 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 @@ -22,8 +22,8 @@ #include #include -#include -#include +#include +#include #include #include 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 8575e4729d7a3..532d770608dd3 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -18,9 +18,9 @@ #include #include -#include -#include +#include #include +#include #include @@ -52,7 +52,7 @@ struct PlannerParam struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double longitudinal_offset_to_first_path_idx; // [m] geometry_msgs::msg::Pose pose; diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 07847a08c1209..c02b5a3852722 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_intersection_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) find_package(OpenCV REQUIRED) diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 0bed7d32f412f..3694a395b3a53 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -18,9 +18,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common fmt geometry_msgs diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index bd2ad1935406b..252d7c2a9f61e 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -406,8 +406,8 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } if (debug_data_.traffic_light_observation) { - const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN; - const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER; + const auto GREEN = autoware_perception_msgs::msg::TrafficLightElement::GREEN; + const auto YELLOW = autoware_perception_msgs::msg::TrafficLightElement::AMBER; const auto [ego, tl_point, id, color] = debug_data_.traffic_light_observation.value(); geometry_msgs::msg::Point tl_point_point; diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index 9002c88354d68..b6ae84aa8488b 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -15,7 +15,7 @@ #ifndef INTERPOLATED_PATH_INFO_HPP_ #define INTERPOLATED_PATH_INFO_HPP_ -#include +#include #include @@ -32,7 +32,7 @@ namespace behavior_velocity_planner::intersection struct InterpolatedPathInfo { /** the interpolated path */ - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; /** discretization interval of interpolation */ double ds{0.0}; /** the intersection lanelet id */ diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 424fb6841eca9..ac32ad553d2b7 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -297,12 +297,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) decision_state_pub_ = node.create_publisher("~/debug/intersection/decision_state", 1); - tl_observation_pub_ = node.create_publisher( + tl_observation_pub_ = node.create_publisher( "~/debug/intersection_traffic_signal", 1); } void IntersectionModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); @@ -356,7 +356,7 @@ void IntersectionModuleManager::launchNewModules( std::function &)> IntersectionModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); @@ -446,7 +446,7 @@ void IntersectionModuleManager::setActivation() } void IntersectionModuleManager::deleteExpiredModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -483,7 +483,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node } void MergeFromPrivateModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); @@ -550,7 +550,7 @@ void MergeFromPrivateModuleManager::launchNewModules( std::function &)> MergeFromPrivateModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp index 46281df2f29c7..7bbc8d51bbe9e 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -23,9 +23,9 @@ #include #include -#include #include #include +#include #include #include @@ -46,10 +46,10 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC // additional for INTERSECTION_OCCLUSION RTCInterface occlusion_rtc_interface_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; @@ -57,10 +57,11 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void sendRTC(const Time & stamp) override; void setActivation() override; /* called from SceneModuleInterface::updateSceneModuleInstances */ - void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; rclcpp::Publisher::SharedPtr decision_state_pub_; - rclcpp::Publisher::SharedPtr tl_observation_pub_; + rclcpp::Publisher::SharedPtr + tl_observation_pub_; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -73,10 +74,10 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface private: MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp index 420031e4df1cf..d2c905673cee9 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -40,7 +40,7 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) tier4_autoware_utils::Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) + const autoware_perception_msgs::msg::Shape & shape) { namespace bg = boost::geometry; const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); @@ -73,7 +73,7 @@ ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_st } void ObjectInfo::initialize( - const autoware_auto_perception_msgs::msg::PredictedObject & object, + const autoware_perception_msgs::msg::PredictedObject & object, std::optional attention_lanelet_opt_, std::optional stopline_opt_) { @@ -250,9 +250,8 @@ std::vector> ObjectInfoManager::allObjects() const } std::optional findPassageInterval( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, - const lanelet::BasicPolygon2d & ego_lane_poly, + 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) { diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp index 77e39637523a9..180496bd6b18d 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -104,7 +104,7 @@ class ObjectInfo public: explicit ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid); - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object() const + const autoware_perception_msgs::msg::PredictedObject & predicted_object() const { return predicted_object_; }; @@ -126,7 +126,7 @@ class ObjectInfo * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline */ void initialize( - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, + const autoware_perception_msgs::msg::PredictedObject & predicted_object, std::optional attention_lanelet_opt, std::optional stopline_opt); @@ -193,7 +193,7 @@ class ObjectInfo const std::string uuid_str; private: - autoware_auto_perception_msgs::msg::PredictedObject predicted_object_; + autoware_perception_msgs::msg::PredictedObject predicted_object_; //! null if the object in intersection_area but not in attention_area std::optional attention_lanelet_opt{std::nullopt}; @@ -283,9 +283,8 @@ class ObjectInfoManager * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically */ std::optional findPassageInterval( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, - const lanelet::BasicPolygon2d & ego_lane_poly, + 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); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 08e2d38a5905f..c27483ea1fa51 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -496,9 +496,8 @@ VisitorSwitch(Ts...) -> VisitorSwitch; template void prepareRTCByDecisionResult( - const T & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const T & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { static_assert("Unsupported type passed to prepareRTCByDecisionResult"); return; @@ -507,7 +506,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( [[maybe_unused]] const intersection::InternalError & result, - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[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) { @@ -517,7 +516,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( [[maybe_unused]] const intersection::OverPassJudge & result, - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[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) { @@ -526,9 +525,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::StuckStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const intersection::StuckStop & 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"), "StuckStop"); const auto closest_idx = result.closest_idx; @@ -547,7 +546,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::YieldStuckStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -562,7 +561,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::NonOccludedCollisionStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -581,7 +580,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::FirstWaitBeforeOcclusion & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -600,7 +599,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::PeekingTowardOcclusion & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -619,7 +618,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::OccludedAbsenceTrafficLight & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -636,7 +635,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::OccludedCollisionStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -654,7 +653,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::Safe & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -674,7 +673,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::FullyPrioritized & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -692,7 +691,7 @@ void prepareRTCByDecisionResult( void IntersectionModule::prepareRTCStatus( const intersection::DecisionResult & decision_result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; double default_distance = std::numeric_limits::lowest(); @@ -713,7 +712,7 @@ template void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const T & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); @@ -727,7 +726,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const intersection::InternalError & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) @@ -742,7 +741,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const intersection::OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) @@ -755,7 +754,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -802,7 +801,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -835,7 +834,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -879,7 +878,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -930,7 +929,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -986,7 +985,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1034,7 +1033,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1088,7 +1087,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1131,7 +1130,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1172,7 +1171,7 @@ void reactRTCApprovalByDecisionResult( void IntersectionModule::reactRTCApproval( const intersection::DecisionResult & decision_result, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) { const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( @@ -1187,7 +1186,7 @@ void IntersectionModule::reactRTCApproval( bool IntersectionModule::isGreenSolidOn() const { - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; if (!last_tl_valid_observation_) { return false; @@ -1205,7 +1204,7 @@ bool IntersectionModule::isGreenSolidOn() const IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel() const { - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; auto corresponding_arrow = [&](const TrafficSignalElement & element) { if (turn_direction_ == "straight" && element.shape == TrafficSignalElement::UP_ARROW) { @@ -1293,7 +1292,7 @@ void IntersectionModule::updateTrafficSignalObservation() } IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines) { const auto & current_pose = planner_data_->current_odometry->pose; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8c8874156f07b..b86fd77491f54 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -27,8 +27,8 @@ #include #include -#include #include +#include #include #include @@ -225,13 +225,13 @@ class IntersectionModule : public SceneModuleInterface std::optional> yield_stuck_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; - autoware_auto_perception_msgs::msg::PredictedObjects unsafe_targets; - autoware_auto_perception_msgs::msg::PredictedObjects misjudge_targets; - autoware_auto_perception_msgs::msg::PredictedObjects too_late_detect_targets; - autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects parked_targets; + autoware_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; + autoware_perception_msgs::msg::PredictedObjects unsafe_targets; + autoware_perception_msgs::msg::PredictedObjects misjudge_targets; + autoware_perception_msgs::msg::PredictedObjects too_late_detect_targets; + autoware_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_perception_msgs::msg::PredictedObjects yield_stuck_targets; + autoware_perception_msgs::msg::PredictedObjects parked_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; @@ -513,15 +513,14 @@ class IntersectionModule : public SceneModuleInterface * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const intersection::DecisionResult &, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const intersection::DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); /** * @brief act based on current RTC approval */ void reactRTCApproval( const intersection::DecisionResult & decision_result, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); /** @}*/ private: @@ -569,7 +568,7 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const intersection::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const; + tier4_planning_msgs::msg::PathWithLaneId * original_path) const; /** * @brief generate IntersectionLanelets @@ -639,15 +638,15 @@ class IntersectionModule : public SceneModuleInterface * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() */ std::optional isStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::IntersectionStopLines & intersection_stoplines, const intersection::PathLanelets & path_lanelets) const; bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; bool isTargetYieldStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief check stuck @@ -669,7 +668,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline */ std::optional isYieldStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, const intersection::IntersectionStopLines & intersection_stoplines) const; @@ -726,7 +725,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.occlusion_stopline */ PassJudgeStatus isOverPassJudgeLinesStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines); /** @} */ @@ -739,7 +738,7 @@ class IntersectionModule : public SceneModuleInterface * @{ */ bool isTargetCollisionVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief find the objects on attention_area/intersection_area and update positional information @@ -759,7 +758,7 @@ class IntersectionModule : public SceneModuleInterface void cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, - autoware_auto_perception_msgs::msg::PredictedPath * predicted_path) const; + autoware_perception_msgs::msg::PredictedPath * predicted_path) const; /** * @brief check if there are any objects around the stoplines on the attention areas when ego @@ -790,7 +789,7 @@ class IntersectionModule : public SceneModuleInterface * situation */ std::string generateEgoRiskEvasiveDiagnosis( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const TimeDistanceArray & ego_time_distance_array, const std::vector< std::pair>> & @@ -818,7 +817,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.first_attention_stopline */ TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const; /** @} */ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 8388bc15492a3..131081c5e8ca0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -35,31 +35,30 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; bool IntersectionModule::isTargetCollisionVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { const auto label = object.classification.at(0).label; const auto & p = planner_param_.collision_detection.target_type; - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + if (label == autoware_perception_msgs::msg::ObjectClassification::CAR && p.car) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BUS && p.bus) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { return true; } - if ( - label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { + if (label == autoware_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; @@ -208,8 +207,8 @@ void IntersectionModule::updateObjectInfoManagerCollision( bool safe_under_traffic_control = false; const auto label = predicted_object.classification.at(0).label; const auto expected_deceleration = - (label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || - label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) + (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE) ? planner_param_.collision_detection.ignore_on_amber_traffic_light .object_expected_deceleration.bike : planner_param_.collision_detection.ignore_on_amber_traffic_light @@ -232,7 +231,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( // check the PredictedPath in the ascending order of its confidence to save the safe/unsafe // CollisionKnowledge for most probable path // ========================================================================================== - std::list sorted_predicted_paths; + std::list sorted_predicted_paths; for (unsigned i = 0; i < predicted_object.kinematics.predicted_paths.size(); ++i) { sorted_predicted_paths.push_back(&predicted_object.kinematics.predicted_paths.at(i)); } @@ -400,7 +399,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( void IntersectionModule::cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, - autoware_auto_perception_msgs::msg::PredictedPath * path) const + autoware_perception_msgs::msg::PredictedPath * path) const { const rclcpp::Time current_time = clock_->now(); const auto original_path = path->path; @@ -597,7 +596,7 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( } std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const IntersectionModule::TimeDistanceArray & ego_time_distance_array, const std::vector>> & @@ -811,7 +810,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( } IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 97d05aef26137..9ea5360c3a176 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -65,7 +65,7 @@ lanelet::ConstLanelets getPrevLanelets( // end inclusive lanelet::ConstLanelet generatePathLanelet( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, const double interval) { lanelet::Points3d lefts; @@ -96,7 +96,7 @@ lanelet::ConstLanelet generatePathLanelet( } std::optional> getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const std::vector & polygons, const bool search_forward = true) { @@ -356,7 +356,7 @@ IntersectionModule::generateIntersectionStopLines( const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const intersection::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const + tier4_planning_msgs::msg::PathWithLaneId * original_path) const { const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; const double stopline_margin = planner_param_.common.default_stopline_margin; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index b26f960ec28f9..498a902c032db 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -119,7 +119,7 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; std::optional IntersectionModule::isStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::IntersectionStopLines & intersection_stoplines, const intersection::PathLanelets & path_lanelets) const { @@ -171,62 +171,60 @@ std::optional IntersectionModule::isStuckStatus( } bool IntersectionModule::isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { const auto label = object.classification.at(0).label; const auto & p = planner_param_.stuck_vehicle.target_type; - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + if (label == autoware_perception_msgs::msg::ObjectClassification::CAR && p.car) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BUS && p.bus) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { return true; } - if ( - label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { + if (label == autoware_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; } bool IntersectionModule::isTargetYieldStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { const auto label = object.classification.at(0).label; const auto & p = planner_param_.yield_stuck.target_type; - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + if (label == autoware_perception_msgs::msg::ObjectClassification::CAR && p.car) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BUS && p.bus) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { return true; } - if ( - label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { + if (label == autoware_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; @@ -309,7 +307,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection( } std::optional IntersectionModule::isYieldStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, const intersection::IntersectionStopLines & intersection_stoplines) const { diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index a44b99c97457d..0b783cf2a7ebd 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -19,10 +19,10 @@ #include #include -#include -#include -#include +#include +#include #include +#include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index dfdfb01fb2234..9c492e7a64cde 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -45,8 +45,7 @@ namespace behavior_velocity_planner::util namespace bg = boost::geometry; static std::optional getDuplicatedPointIdx( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & point) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) { for (size_t i = 0; i < path.points.size(); i++) { const auto & p = path.points.at(i).point.pose.position; @@ -61,8 +60,7 @@ static std::optional getDuplicatedPointIdx( } std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); @@ -75,8 +73,7 @@ std::optional insertPointIndex( // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) int insert_idx = closest_idx; - autoware_auto_planning_msgs::msg::PathPointWithLaneId inserted_point = - inout_path->points.at(closest_idx); + tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { ++insert_idx; } else { @@ -94,8 +91,7 @@ std::optional insertPointIndex( } bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, - const std::set & ids) + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids) { for (const auto & pid : p.lane_ids) { if (ids.find(pid) != ids.end()) { @@ -106,7 +102,7 @@ bool hasLaneIds( } std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) + const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) { bool found = false; size_t start = 0; @@ -183,7 +179,7 @@ getFirstPointInsidePolygonsByFootprint( } std::optional getFirstPointInsidePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward) { @@ -316,7 +312,7 @@ mergeLaneletsByTopologicalSort( } bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { @@ -327,7 +323,7 @@ bool isOverTargetIndex( } bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { @@ -362,7 +358,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { intersection::InterpolatedPathInfo interpolated_path_info; @@ -378,7 +374,7 @@ std::optional generateInterpolatedPath( } geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( - const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state) + const autoware_perception_msgs::msg::PredictedObjectKinematics & obj_state) { if (obj_state.initial_twist_with_covariance.twist.linear.x >= 0) { return obj_state.initial_pose_with_covariance.pose; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index e37bb3ee88cc1..878253e6943a7 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -38,16 +38,14 @@ namespace behavior_velocity_planner::util * @return if insertion was successful return the inserted point index */ std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); /** * @brief check if a PathPointWithLaneId contains any of the given lane ids */ bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, - const std::set & ids); + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); /** * @brief find the first contiguous interval of the path points that contains the specified lane ids @@ -55,7 +53,7 @@ bool hasLaneIds( * found, returns the pair (start-1, end) */ std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); + const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** * @brief return the index of the first point which is inside the given polygon @@ -63,7 +61,7 @@ std::optional> findLaneIdsInterval( * @param[in] search_forward flag for search direction */ std::optional getFirstPointInsidePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward = true); @@ -75,7 +73,7 @@ std::optional getFirstPointInsidePolygon( * @return true if ego is over the target_idx */ bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); /** @@ -86,7 +84,7 @@ bool isOverTargetIndex( * @return true if ego is over the target_idx */ bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); std::optional getIntersectionArea( @@ -102,11 +100,11 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); */ std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( - const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); + const autoware_perception_msgs::msg::PredictedObjectKinematics & obj_state); /** * @brief this function sorts the set of lanelets topologically using topological sort and merges diff --git a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt b/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt index ef3073df56c22..265b5b637e7ce 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_no_drivable_lane_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_no_drivable_lane_module/package.xml index 061cf8d367ba9..79eea68142cc8 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/package.xml @@ -19,7 +19,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs 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 f1fa4c47e951b..0b7a8a8c28dd5 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -32,7 +32,7 @@ NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) } void NoDrivableLaneModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -56,7 +56,7 @@ void NoDrivableLaneModuleManager::launchNewModules( std::function &)> NoDrivableLaneModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); 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 2b3b80510c9e4..90455bd4b1c62 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class NoDrivableLaneModuleManager : public SceneModuleManagerInterface private: NoDrivableLaneModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class NoDrivableLaneModulePlugin : public PluginWrapper 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 51d3339339051..fb90e023fedcd 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -26,7 +26,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class NoDrivableLaneModule : public SceneModuleInterface { 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 1cd527d93ef74..11953fd5dd177 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp @@ -15,7 +15,7 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include +#include #include #include @@ -33,8 +33,8 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; // the status of intersection between path and no drivable lane polygon struct PathWithNoDrivableLanePolygonIntersection diff --git a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt index 4d14228c4685c..ba3fd0e4f71dd 100644 --- a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_no_stopping_area_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml index a9c21f1aba189..0991b37120a6f 100644 --- a/planning/behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -17,8 +17,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs 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 46cc224ea0f6b..faa265afe1559 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -52,7 +52,7 @@ NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) } void NoStoppingAreaModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & m : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -76,7 +76,7 @@ void NoStoppingAreaModuleManager::launchNewModules( std::function &)> NoStoppingAreaModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); 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 f7a9a5433e900..baf5901ccc124 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC private: NoStoppingAreaModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class NoStoppingAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 7b886fea09b34..3f2e7581a4d14 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -56,8 +56,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( } boost::optional NoStoppingAreaModule::getStopLineGeometry2d( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double stop_line_margin) const + const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const { // get stop line from map { @@ -216,8 +215,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( const Polygon2d & poly, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & - predicted_obj_arr_ptr) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr) { // stuck points by predicted objects for (const auto & object : predicted_obj_arr_ptr->objects) { @@ -250,7 +248,7 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( return false; } bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) { const double stop_vel = std::numeric_limits::min(); // stuck points by stop line @@ -278,14 +276,14 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( } Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const double margin, const double extra_dist) const { Polygon2d ego_area; // open polygon double dist_from_start_sum = 0.0; const double interpolation_interval = 0.5; bool is_in_area = false; - autoware_auto_planning_msgs::msg::PathWithLaneId interpolated_path; + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger_)) { return ego_area; } @@ -350,19 +348,19 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } bool NoStoppingAreaModule::isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { if ( object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + autoware_perception_msgs::msg::ObjectClassification::CAR || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + autoware_perception_msgs::msg::ObjectClassification::BUS || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + autoware_perception_msgs::msg::ObjectClassification::TRUCK || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + autoware_perception_msgs::msg::ObjectClassification::TRAILER || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { return true; } return false; @@ -404,13 +402,13 @@ bool NoStoppingAreaModule::isStoppable( } void NoStoppingAreaModule::insertStopPoint( - autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) { size_t insert_idx = static_cast(stop_point.first + 1); const auto stop_pose = stop_point.second; // To PathPointWithLaneId - autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(insert_idx); stop_point_with_lane_id.point.pose = stop_pose; stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; 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 84f260e5e2c2b..62ec0b88b328e 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 @@ -24,10 +24,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -101,7 +101,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return true if the object has a target type */ bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief Check if there is a stopped vehicle in stuck vehicle detect area. @@ -111,8 +111,7 @@ class NoStoppingAreaModule : public SceneModuleInterface */ bool checkStuckVehiclesInNoStoppingArea( const Polygon2d & poly, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & - predicted_obj_arr_ptr); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr); /** * @brief Check if there is a stop line in "stop line detect area". @@ -121,7 +120,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return true if exists */ bool checkStopLinesInNoStoppingArea( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly); + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly); /** * @brief Calculate the polygon of the path from the ego-car position to the end of the @@ -133,7 +132,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return generated polygon */ Polygon2d generateEgoNoStoppingAreaLanePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const; /** @@ -144,8 +143,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return generated stop line */ boost::optional getStopLineGeometry2d( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double stop_line_margin) const; + const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const; /** * @brief Calculate if it's possible for ego-vehicle to stop before area consider jerk limit @@ -162,7 +160,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @param stop_point stop line point on the lane */ void insertStopPoint( - autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); // Key Feature const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem_; diff --git a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt index 7a062ee854421..8b327291a60f1 100644 --- a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_occlusion_spot_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_occlusion_spot_module/package.xml index cb5bd744edfa8..5817c2203cc5d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_occlusion_spot_module/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs grid_map_ros 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 c9dbe13474b06..710f671df609f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -25,9 +25,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -55,8 +55,8 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; namespace grid_utils { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index f257012c05519..64adc112aab2f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -117,7 +117,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) } void OcclusionSpotModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -130,7 +130,7 @@ void OcclusionSpotModuleManager::launchNewModules( std::function &)> OcclusionSpotModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index c153a727b8a7e..0955e4ae9aab5 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -23,11 +23,11 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include #include @@ -53,10 +53,10 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class OcclusionSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index f22cedbe67c8f..a650a6fa5b39f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -94,8 +94,8 @@ bool buildDetectionAreaPolygon( } void calcSlowDownPointsForPossibleCollision( - const int closest_idx, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double offset, std::vector & possible_collisions) + const int closest_idx, const tier4_planning_msgs::msg::PathWithLaneId & path, const double offset, + std::vector & possible_collisions) { if (possible_collisions.empty()) { return; 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 7f495f042d7f8..2b6d89680cd37 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 @@ -23,11 +23,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -48,19 +48,19 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::BasicLineString2d; using lanelet::BasicPoint2d; using lanelet::BasicPolygon2d; using lanelet::LaneletMapPtr; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using DetectionAreaIdx = std::optional>; using BasicPolygons2d = std::vector; 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 9a0278bfad5e5..8d7e9d2fdedd5 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 @@ -17,7 +17,7 @@ #include "occlusion_spot_utils.hpp" -#include +#include #include #include 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 8ac3fe26c1c33..1f5ca1bab904e 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 @@ -42,7 +42,7 @@ namespace { namespace utils = behavior_velocity_planner::occlusion_spot_utils; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; std::vector extractStuckVehicle( const std::vector & vehicles, const double stop_velocity) { 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 2cd7ea3582452..b7da7c073cbd9 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 @@ -22,11 +22,11 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include 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 85ae85f1895d9..05e73855f2642 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 @@ -24,11 +24,11 @@ using Point = geometry_msgs::msg::Point; using Vector3 = geometry_msgs::msg::Vector3; -using DynamicObjects = autoware_auto_perception_msgs::msg::PredictedObjects; -using DynamicObject = autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using DynamicObjects = autoware_perception_msgs::msg::PredictedObjects; +using DynamicObject = autoware_perception_msgs::msg::PredictedObject; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using tier4_planning_msgs::msg::PathWithLaneId; TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) { @@ -41,8 +41,7 @@ TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) std::vector possible_collisions; size_t num = 2000; // make a path with 2000 points from x=0 to x=4 - autoware_auto_planning_msgs::msg::PathWithLaneId path = - test::generatePath(0.0, 3.0, 4.0, 3.0, num); + tier4_planning_msgs::msg::PathWithLaneId path = test::generatePath(0.0, 3.0, 4.0, 3.0, num); // make 2000 possible collision from x=0 to x=10 test::generatePossibleCollisions(possible_collisions, 0.0, 3.0, 4.0, 3.0, num); 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 cf24be4cc0b5c..f7bb3d4b1ad6f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -19,22 +19,22 @@ #include -#include -#include +#include +#include #include namespace test { -inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( +inline tier4_planning_msgs::msg::PathWithLaneId generatePath( double x0, double y0, double x, double y, int nb_points) { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; double x_step = (x - x0) / (nb_points - 1); double y_step = (y - y0) / (nb_points - 1); for (int i = 0; i < nb_points; ++i) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + tier4_planning_msgs::msg::PathPointWithLaneId point{}; point.point.pose.position.x = x0 + x_step * i; point.point.pose.position.y = y0 + y_step * i; point.point.pose.position.z = 0.0; @@ -88,7 +88,7 @@ inline void generatePossibleCollisions( intersection_pose.position.x = y0 + y_step * i; // collision path point - autoware_auto_planning_msgs::msg::PathPoint collision_with_margin{}; + autoware_planning_msgs::msg::PathPoint collision_with_margin{}; collision_with_margin.pose.position.x = x0 + x_step * i + lon; collision_with_margin.pose.position.y = y0 + y_step * i; diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt index 8ce1a334c4b35..e8191f9b632d5 100644 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_out_of_lane_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml index b61441132969c..d3742ea7d305f 100644 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -14,8 +14,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs lanelet2_extension 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 489bb3f5abc78..862ca351a118a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -113,8 +113,8 @@ void add_lanelet_markers( void add_range_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx, - const double z, const size_t prev_nb) + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx, const double z, + const size_t prev_nb) { auto debug_marker = get_base_marker(); debug_marker.ns = "ranges"; 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 2b6b65638ec40..0802ae78d7a26 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp @@ -62,7 +62,7 @@ void add_lanelet_markers( /// @param [in] prev_nb previous number of markers (used to delete the extra ones) void add_range_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, + 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 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 e948bd74eba45..126c75c2f80b9 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -61,7 +61,7 @@ bool object_is_incoming( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const double dist_buffer, const rclcpp::Logger & logger) { @@ -155,7 +155,7 @@ std::optional> object_time_to_range( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger) { const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; 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 0612cc041c1ef..4f2b0a6dad89b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include @@ -53,7 +53,7 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const double dist_buffer, const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit @@ -67,7 +67,7 @@ std::optional> object_time_to_range( /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit. std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger); /// @brief decide whether an object is coming in the range at the same time as ego /// @details the condition depends on the mode (threshold, intervals, ttc) 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 de12334b91a19..80cd106bf52ab 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 @@ -27,7 +27,7 @@ namespace behavior_velocity_planner::out_of_lane { void cut_predicted_path_beyond_line( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) { auto stop_line_idx = 0UL; @@ -57,7 +57,7 @@ void cut_predicted_path_beyond_line( } std::optional find_next_stop_line( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) + const autoware_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) { lanelet::ConstLanelets lanelets; lanelet::BasicLineString2d query_line; @@ -81,8 +81,8 @@ std::optional find_next_stop_line( } void cut_predicted_path_beyond_red_lights( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const PlannerData & planner_data, const double object_front_overhang) + autoware_perception_msgs::msg::PredictedPath & predicted_path, const PlannerData & planner_data, + const double object_front_overhang) { const auto stop_line = find_next_stop_line(predicted_path, planner_data); if (stop_line) { @@ -95,15 +95,15 @@ void cut_predicted_path_beyond_red_lights( } } -autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { - autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + autoware_perception_msgs::msg::PredictedObjects filtered_objects; filtered_objects.header = planner_data.predicted_objects->header; for (const auto & object : planner_data.predicted_objects->objects) { const auto is_pedestrian = std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; }) != object.classification.end(); if (is_pedestrian) continue; 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 bb6b5f4d00005..be3e8809d2e3d 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 @@ -28,7 +28,7 @@ namespace behavior_velocity_planner::out_of_lane /// @param [in] stop_line stop line used for cutting /// @param [in] object_front_overhang extra distance to cut ahead of the stop line void cut_predicted_path_beyond_line( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); /// @brief find the next red light stop line along the given path @@ -36,21 +36,21 @@ void cut_predicted_path_beyond_line( /// @param [in] planner_data planner data with stop line information /// @return the first red light stop line found along the path (if any) std::optional find_next_stop_line( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); + const autoware_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); /// @brief cut predicted path beyond stop lines of red lights /// @param [inout] predicted_path predicted path to cut /// @param [in] planner_data planner data to get the map and traffic light information void cut_predicted_path_beyond_red_lights( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const PlannerData & planner_data, const double object_front_overhang); + autoware_perception_msgs::msg::PredictedPath & predicted_path, const PlannerData & planner_data, + const double object_front_overhang); /// @brief filter predicted objects and their predicted paths /// @param [in] planner_data planner data /// @param [in] ego_data ego data /// @param [in] params parameters /// @return filtered predicted objects -autoware_auto_perception_msgs::msg::PredictedObjects filter_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 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 7d764722405af..840054d92252f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -82,8 +82,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.right_offset = vehicle_info.min_lateral_offset_m; } -void OutOfLaneModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void OutOfLaneModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -95,7 +94,7 @@ void OutOfLaneModuleManager::launchNewModules( std::function &)> OutOfLaneModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; 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 c8ef397913c8f..9da1751576a7f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp @@ -22,11 +22,11 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include #include @@ -51,10 +51,10 @@ class OutOfLaneModuleManager : public SceneModuleManagerInterface PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class OutOfLaneModulePlugin : public PluginWrapper 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 6133bb1ea0d6e..1d51c45c6afd1 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 @@ -21,9 +21,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include 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 ff690699ee638..cb2945f8b32b8 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -17,9 +17,9 @@ #include -#include -#include +#include #include +#include #include @@ -99,7 +99,7 @@ struct Slowdown struct SlowdownToInsert { Slowdown slowdown; - autoware_auto_planning_msgs::msg::PathWithLaneId::_points_type::value_type point; + tier4_planning_msgs::msg::PathWithLaneId::_points_type::value_type point; }; /// @brief bound of an overlap range (either the first, or last bound) @@ -135,7 +135,7 @@ struct OverlapRange std::vector overlaps; std::optional decision; RangeTimes times; - std::optional object{}; + std::optional object{}; } debug; }; using OverlapRanges = std::vector; @@ -172,7 +172,7 @@ struct OtherLane /// @brief data related to the ego vehicle struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double velocity{}; // [m/s] double max_decel{}; // [m/s²] @@ -184,7 +184,7 @@ struct DecisionInputs { OverlapRanges ranges{}; EgoData ego_data; - autoware_auto_perception_msgs::msg::PredictedObjects objects{}; + autoware_perception_msgs::msg::PredictedObjects objects{}; std::shared_ptr route_handler{}; lanelet::ConstLanelets lanelets{}; }; @@ -201,7 +201,7 @@ struct DebugData lanelet::ConstLanelets path_lanelets; lanelet::ConstLanelets ignored_lanelets; lanelet::ConstLanelets other_lanelets; - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; size_t first_path_idx; size_t prev_footprints = 0; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 1511d430ddd0c..51511b94f3e33 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -17,13 +17,13 @@ #include "route_handler/route_handler.hpp" +#include #include -#include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -66,7 +66,7 @@ struct PlannerData geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; static constexpr double velocity_buffer_time_sec = 10.0; std::deque velocity_buffer; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; // occupancy grid nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; @@ -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 diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp index 624da6b170d06..dcdb4a7052cc0 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -30,10 +30,10 @@ class PluginInterface public: virtual ~PluginInterface() = default; virtual void init(rclcpp::Node & node) = 0; - virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) = 0; + virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) = 0; virtual void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::optional getFirstStopPathPointIndex() = 0; virtual const char * getModuleName() = 0; }; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp index 7aa3e6bfef9ab..abb14dd8b2356 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -28,13 +28,13 @@ class PluginWrapper : public PluginInterface { public: void init(rclcpp::Node & node) override { scene_manager_ = std::make_unique(node); } - void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override + void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override { scene_manager_->plan(path); }; void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override + const tier4_planning_msgs::msg::PathWithLaneId & path) override { scene_manager_->updateSceneModuleInstances(planner_data, path); } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index 3a2b474e3d730..3e7992207f3f1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -26,9 +26,9 @@ #include #include -#include -#include +#include #include +#include #include #include #include @@ -48,7 +48,6 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; using motion_utils::PlanningBehavior; using motion_utils::VelocityFactor; @@ -58,6 +57,7 @@ using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::getOrDeclareParameter; using tier4_debug_msgs::msg::Float64Stamped; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_rtc_msgs::msg::Module; @@ -67,10 +67,10 @@ using unique_identifier_msgs::msg::UUID; struct ObjectOfInterest { geometry_msgs::msg::Pose pose; - autoware_auto_perception_msgs::msg::Shape shape; + autoware_perception_msgs::msg::Shape shape; ColorName color; ObjectOfInterest( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name) : pose(pose), shape(shape), color(color_name) { @@ -144,14 +144,14 @@ class SceneModuleInterface void syncActivation() { setActivation(isSafe()); } void setObjectsOfInterestData( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name) { objects_of_interest_.emplace_back(pose, shape, color_name); } size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; }; class SceneModuleManagerInterface @@ -167,22 +167,19 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const tier4_planning_msgs::msg::PathWithLaneId & path); - virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) - { - modifyPathVelocity(path); - } + virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } protected: - virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path); + virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path); - virtual void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::function &)> - getModuleExpiredFunction(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path); bool isModuleRegistered(const int64_t module_id) { @@ -194,7 +191,7 @@ class SceneModuleManagerInterface void unregisterModule(const std::shared_ptr & scene_module); size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; std::set> scene_modules_; std::set registered_module_id_set_; @@ -210,7 +207,7 @@ class SceneModuleManagerInterface rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; - rclcpp::Publisher::SharedPtr pub_debug_path_; + rclcpp::Publisher::SharedPtr pub_debug_path_; rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_velocity_factor_; @@ -226,7 +223,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface SceneModuleManagerInterfaceWithRTC( rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); - void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; + void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; protected: RTCInterface rtc_interface_; @@ -261,7 +258,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void publishObjectsOfInterestMarker(); - void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) { diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 98cf5114fb0e7..c9d292536ac13 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -159,8 +159,7 @@ std::optional findOffsetSegment( } std::optional findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, - const double offset); + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset); template geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment) @@ -192,7 +191,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse } std::optional createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const size_t lane_id, const double margin, const double vehicle_offset); } // namespace arc_lane_utils diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 0f3cc7203ef1f..bf238ecad55cb 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -17,12 +17,12 @@ #include -#include -#include -#include +#include +#include #include #include #include +#include #include #include @@ -37,13 +37,13 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( geometry_msgs::msg::PoseWithCovarianceStamped, double, cs::cartesian, pose.pose.position.x, pose.pose.position.y, pose.pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - autoware_auto_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, - pose.position.y, pose.position.z) + autoware_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, pose.position.y, + pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - autoware_auto_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, - point.pose.position.x, point.pose.position.y, point.pose.position.z) + tier4_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, point.pose.position.x, + point.pose.position.y, point.pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - autoware_auto_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, + autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, pose.position.y, pose.position.z) namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp index 98df29d1c44c0..ab44af265fbaa 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp @@ -17,10 +17,10 @@ #include -#include -#include +#include #include #include +#include #include #include @@ -35,11 +35,11 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createPathMarkerArray( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createObjectsMarkerArray( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createPointsMarkerArray( diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 667c915ac1d7f..55a82db1ae390 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -17,22 +17,22 @@ #include -#include -#include +#include +#include #include namespace behavior_velocity_planner { bool splineInterpolate( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, - autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); -autoware_auto_planning_msgs::msg::Path interpolatePath( - const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval); -autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( - const autoware_auto_planning_msgs::msg::Path & path); -autoware_auto_planning_msgs::msg::Path filterStopPathPoint( - const autoware_auto_planning_msgs::msg::Path & path); + const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, + tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); +autoware_planning_msgs::msg::Path interpolatePath( + const autoware_planning_msgs::msg::Path & path, const double length, const double interval); +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 #endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index 2184be4fbd2fe..2aadb7883a857 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -17,10 +17,10 @@ #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -28,9 +28,9 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index e042338c53485..4ef4bb91a295d 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -17,10 +17,10 @@ #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -53,7 +53,7 @@ struct DetectionRange struct TrafficSignalStamped { builtin_interfaces::msg::Time stamp; - autoware_perception_msgs::msg::TrafficSignal signal; + autoware_perception_msgs::msg::TrafficLightGroup signal; }; using Pose = geometry_msgs::msg::Pose; @@ -62,16 +62,16 @@ using LineString2d = tier4_autoware_utils::LineString2d; using Polygon2d = tier4_autoware_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; namespace planning_utils { size_t calcSegmentIndexFromPointIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx); // create detection area from given range return false if creation failure bool createDetectionAreaPolygons( @@ -96,7 +96,7 @@ inline int64_t bitShift(int64_t original_id) bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const tier4_planning_msgs::msg::PathWithLaneId & path); Polygon2d generatePathPolygon( const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); double calcJudgeLineDistWithAccLimit( @@ -210,9 +210,8 @@ std::set getLaneIdSetOnPath( const geometry_msgs::msg::Pose & current_pose); bool isOverLine( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, - const double offset = 0.0); + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const double offset = 0.0); std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 784b7cabfe9ad..ba847d8b1f853 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -20,17 +20,16 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 + autoware_map_msgs autoware_perception_msgs + autoware_planning_msgs + autoware_velocity_smoother diagnostic_msgs eigen geometry_msgs interpolation lanelet2_extension motion_utils - motion_velocity_smoother nav_msgs objects_of_interest_marker_interface pcl_conversions diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index eb43e45d55711..3092d33418c8b 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -38,7 +38,7 @@ SceneModuleInterface::SceneModuleInterface( } size_t SceneModuleInterface::findEgoSegmentIndex( - const std::vector & points) const + const std::vector & points) const { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -57,7 +57,7 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); } if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( + pub_debug_path_ = node.create_publisher( std::string("~/debug/path_with_lane_id/") + module_name, 1); } pub_virtual_wall_ = node.create_publisher( @@ -74,7 +74,7 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( } size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const + const std::vector & points) const { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -83,7 +83,7 @@ size_t SceneModuleManagerInterface::findEgoSegmentIndex( void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { planner_data_ = planner_data; @@ -92,7 +92,7 @@ void SceneModuleManagerInterface::updateSceneModuleInstances( } void SceneModuleManagerInterface::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path) + tier4_planning_msgs::msg::PathWithLaneId * path) { StopWatch stop_watch; stop_watch.tic("Total"); @@ -145,7 +145,7 @@ void SceneModuleManagerInterface::modifyPathVelocity( pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { - autoware_auto_planning_msgs::msg::PathWithLaneId debug_path; + tier4_planning_msgs::msg::PathWithLaneId debug_path; debug_path.header = path->header; debug_path.points = path->points; pub_debug_path_->publish(debug_path); @@ -156,7 +156,7 @@ void SceneModuleManagerInterface::modifyPathVelocity( } void SceneModuleManagerInterface::deleteExpiredModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -198,8 +198,7 @@ SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( { } -void SceneModuleManagerInterfaceWithRTC::plan( - autoware_auto_planning_msgs::msg::PathWithLaneId * path) +void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) { setActivation(); modifyPathVelocity(path); @@ -262,7 +261,7 @@ void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() } void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); diff --git a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index 9409dddb678db..e5705b1367e0e 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -98,8 +98,7 @@ std::optional checkCollision( } std::optional findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, - const double offset) + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset) { if (offset >= 0) { return findForwardOffsetSegment(path, index, offset); @@ -109,7 +108,7 @@ std::optional findOffsetSegment( } std::optional createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const size_t lane_id, const double margin, const double vehicle_offset) { // Find collision segment diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp index 6ff2ef53176c4..00d746c56db85 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp @@ -54,7 +54,7 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( } visualization_msgs::msg::MarkerArray createPathMarkerArray( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b) { @@ -83,7 +83,7 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( } visualization_msgs::msg::MarkerArray createObjectsMarkerArray( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; diff --git a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp index 59c9e4861fbfd..fe956e9be9512 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -26,8 +26,8 @@ constexpr double DOUBLE_EPSILON = 1e-6; namespace behavior_velocity_planner { bool splineInterpolate( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, - autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger) + const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, + tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger) { if (input.points.size() < 2) { RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1."); @@ -47,8 +47,8 @@ bool splineInterpolate( * is the velocity of the closest point for the input "sub-path" which consists of the points before * the interpolated point. */ -autoware_auto_planning_msgs::msg::Path interpolatePath( - const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval) +autoware_planning_msgs::msg::Path interpolatePath( + const autoware_planning_msgs::msg::Path & path, const double length, const double interval) { const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("path_utilization")}; @@ -125,10 +125,10 @@ autoware_auto_planning_msgs::msg::Path interpolatePath( return motion_utils::resamplePath(path, s_out); } -autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( - const autoware_auto_planning_msgs::msg::Path & path) +autoware_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_planning_msgs::msg::Path & path) { - autoware_auto_planning_msgs::msg::Path filtered_path; + autoware_planning_msgs::msg::Path filtered_path; const double epsilon = 0.01; size_t latest_id = 0; @@ -153,10 +153,10 @@ autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( return filtered_path; } -autoware_auto_planning_msgs::msg::Path filterStopPathPoint( - const autoware_auto_planning_msgs::msg::Path & path) +autoware_planning_msgs::msg::Path filterStopPathPoint( + const autoware_planning_msgs::msg::Path & path) { - autoware_auto_planning_msgs::msg::Path filtered_path = path; + autoware_planning_msgs::msg::Path filtered_path = path; bool found_stop = false; for (size_t i = 0; i < filtered_path.points.size(); ++i) { if (std::fabs(filtered_path.points.at(i).longitudinal_velocity_mps) < 0.01) { diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 314281a954b49..ecc314bca2009 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -15,13 +15,13 @@ // #include #include "motion_utils/trajectory/conversion.hpp" +#include #include #include -#include #include -#include #include +#include #include @@ -35,10 +35,10 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; @@ -55,8 +55,7 @@ bool smoothPath( const auto & smoother = planner_data->velocity_smoother_; auto trajectory = - motion_utils::convertToTrajectoryPoints( - in_path); + motion_utils::convertToTrajectoryPoints(in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); const auto traj_steering_rate_limited = @@ -83,7 +82,7 @@ bool smoothPath( traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - motion_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); diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp index a3d1aab4f9b9f..b724d01346f1e 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include @@ -39,7 +39,7 @@ namespace { size_t calcPointIndexFromSegmentIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t seg_idx) { const size_t prev_point_idx = seg_idx; @@ -54,7 +54,7 @@ size_t calcPointIndexFromSegmentIndex( return next_point_idx; } -using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::PathPoint; PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio) { @@ -93,7 +93,7 @@ namespace behavior_velocity_planner { namespace planning_utils { -using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::PathPoint; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; using motion_utils::validateNonEmpty; @@ -105,7 +105,7 @@ using tier4_autoware_utils::createQuaternionFromYaw; using tier4_autoware_utils::getPoint; size_t calcSegmentIndexFromPointIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx) { if (idx == 0) { @@ -304,7 +304,7 @@ bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.size() == 0) { return geometry_msgs::msg::Pose{}; @@ -605,9 +605,8 @@ std::vector getSubsequentLaneIdsSetOnPath( // TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex bool isOverLine( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, - const double offset) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const double offset) { return motion_utils::calcSignedArcLength(path.points, self_pose.position, line_pose.position) + offset < diff --git a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp b/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp index f43450800b46e..fbc5f5d709c5c 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp @@ -82,8 +82,8 @@ TEST(smoothDeceleration, calculateMaxSlowDownVelocity) TEST(specialInterpolation, specialInterpolation) { - using autoware_auto_planning_msgs::msg::Path; - using autoware_auto_planning_msgs::msg::PathPoint; + 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/behavior_velocity_planner_common/test/src/utils.hpp index 1328edb6f6027..4dbf4c73fcd82 100644 --- a/planning/behavior_velocity_planner_common/test/src/utils.hpp +++ b/planning/behavior_velocity_planner_common/test/src/utils.hpp @@ -15,23 +15,23 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ -#include -#include #include +#include +#include #include namespace test { -inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( +inline tier4_planning_msgs::msg::PathWithLaneId generatePath( double x0, double y0, double x, double y, int nb_points) { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; double x_step = (x - x0) / (nb_points - 1); double y_step = (y - y0) / (nb_points - 1); for (int i = 0; i < nb_points; ++i) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + tier4_planning_msgs::msg::PathPointWithLaneId point{}; point.point.pose.position.x = x0 + x_step * i; point.point.pose.position.y = y0 + y_step * i; point.point.pose.position.z = 0.0; diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_run_out_module/CMakeLists.txt index 21758c4d0198c..3ee589ba69740 100644 --- a/planning/behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_run_out_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_run_out_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index 7c79d1c288a0d..d3e2d813c3799 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -71,14 +71,14 @@ This module can handle multiple types of obstacles by creating abstracted dynami Abstracted obstacle data has following information. -| Name | Type | Description | -| ---------------- | ----------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------- | -| pose | `geometry_msgs::msg::Pose` | pose of the obstacle | -| classifications | `std::vector` | classifications with probability | -| shape | `autoware_auto_perception_msgs::msg::Shape` | shape of the obstacle | -| predicted_paths | `std::vector` | predicted paths with confidence. this data doesn't have time step because we use minimum and maximum velocity instead. | -| min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | -| max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | +| Name | Type | Description | +| ---------------- | ------------------------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------- | +| pose | `geometry_msgs::msg::Pose` | pose of the obstacle | +| classifications | `std::vector` | classifications with probability | +| shape | `autoware_perception_msgs::msg::Shape` | shape of the obstacle | +| predicted_paths | `std::vector` | predicted paths with confidence. this data doesn't have time step because we use minimum and maximum velocity instead. | +| min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | +| max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | Enter the maximum/minimum velocity of the object as a parameter, adding enough margin to the expected velocity. This parameter is used to create polygons for [collision detection](.#Collision-detection). diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index a0b15570f48f2..40e55b3de1a17 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -19,8 +19,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_crosswalk_module behavior_velocity_planner_common eigen diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 9b94c8b681942..edf1d8cf48b8e 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -321,7 +321,7 @@ double convertDurationToDouble(const builtin_interfaces::msg::Duration & duratio // Create a path leading up to a specified prediction time std::vector createPathToPredictionTime( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time) + const autoware_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time) { // Calculate the number of poses to include based on the prediction time and the time step between // poses diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index f020da2abd045..c7d8ef674aafd 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -44,17 +44,17 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; using run_out_utils::DynamicObstacle; using run_out_utils::DynamicObstacleData; using run_out_utils::DynamicObstacleParam; using run_out_utils::PlannerParam; using run_out_utils::PredictedPath; -using PathPointsWithLaneId = std::vector; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; +using PathPointsWithLaneId = std::vector; /** * @brief base class for creating dynamic obstacles from multiple types of input diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 1c85a22f57bf6..fb6a70070749a 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -146,8 +146,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) setDynamicObstacleCreator(node, debug_ptr_); } -void RunOutModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) { return; @@ -162,8 +161,7 @@ void RunOutModuleManager::launchNewModules( } std::function &)> -RunOutModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +RunOutModuleManager::getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) { return [&path]([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { diff --git a/planning/behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_run_out_module/src/manager.hpp index 4dd66ad45898b..ad35caf98149a 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_run_out_module/src/manager.hpp @@ -37,10 +37,10 @@ class RunOutModuleManager : public SceneModuleManagerInterface std::shared_ptr debug_ptr_; std::unique_ptr dynamic_obstacle_creator_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; void setDynamicObstacleCreator(rclcpp::Node & node, std::shared_ptr & debug_ptr); }; diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.cpp b/planning/behavior_velocity_run_out_module/src/path_utils.cpp index 1cec20297683c..be7db13cd3ca3 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.cpp @@ -18,7 +18,7 @@ namespace behavior_velocity_planner::run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & src_point, const std::vector & target_points) { diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.hpp b/planning/behavior_velocity_run_out_module/src/path_utils.hpp index 92aca946c13ef..8867778520bc7 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.hpp @@ -15,8 +15,8 @@ #ifndef PATH_UTILS_HPP_ #define PATH_UTILS_HPP_ -#include #include +#include #include #include @@ -31,7 +31,7 @@ namespace run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & src_point, const std::vector & target_points); diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index da826e1cf0cf6..de50b3161ea6f 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -743,7 +743,7 @@ std::optional RunOutModule::calcStopPoint( bool RunOutModule::insertStopPoint( const std::optional stop_point, - autoware_auto_planning_msgs::msg::PathWithLaneId & path) + tier4_planning_msgs::msg::PathWithLaneId & path) { // no stop point if (!stop_point) { @@ -764,7 +764,7 @@ bool RunOutModule::insertStopPoint( } // to PathPointWithLaneId - autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(nearest_seg_idx); stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); @@ -880,7 +880,7 @@ void RunOutModule::insertApproachingVelocity( // to PathPointWithLaneId // use lane id of point behind inserted point - autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = output_path.points.at(nearest_seg_idx_stop); stop_point_with_lane_id.point.pose = *stop_point; diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index 3db6051ab36e7..2f797a44d06f7 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -32,12 +32,12 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; using run_out_utils::PoseWithRange; using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; class RunOutModule : public SceneModuleInterface @@ -124,7 +124,7 @@ class RunOutModule : public SceneModuleInterface bool insertStopPoint( const std::optional stop_point, - autoware_auto_planning_msgs::msg::PathWithLaneId & path); + tier4_planning_msgs::msg::PathWithLaneId & path); void insertVelocityForState( const std::optional & dynamic_obstacle, const PlannerData planner_data, diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp index f17f4c7251ea4..c6eda901a1424 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -54,7 +54,7 @@ Polygon2d createBoostPolyFromMsg(const std::vector & std::uint8_t getHighestProbLabel(const std::vector & classification) { - std::uint8_t label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + std::uint8_t label = autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; float highest_prob = 0.0; for (const auto & _class : classification) { if (highest_prob < _class.probability) { diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 51b460482458f..9208e57dcdc7f 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -21,9 +21,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -34,18 +34,18 @@ namespace behavior_velocity_planner namespace run_out_utils { namespace bg = boost::geometry; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::PathPoint; using tier4_autoware_utils::Box2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::PathWithLaneId; using vehicle_info_util::VehicleInfo; -using PathPointsWithLaneId = std::vector; +using PathPointsWithLaneId = std::vector; struct CommonParam { double normal_min_jerk; // min jerk limit for mild stop [m/sss] diff --git a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt index a96eb3398e5c0..f5d3a7078be9c 100644 --- a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt +++ b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_speed_bump_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_speed_bump_module/package.xml index 3dd16f2fd792a..9b1de66c95c92 100644 --- a/planning/behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_speed_bump_module/package.xml @@ -15,7 +15,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index 5949357c90ff3..fcd000699b7ff 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -53,8 +53,7 @@ SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) static_cast(getOrDeclareParameter(node, ns + ".max_speed")); } -void SpeedBumpModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & speed_bump_with_lane_id : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -71,7 +70,7 @@ void SpeedBumpModuleManager::launchNewModules( std::function &)> SpeedBumpModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto speed_bump_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_speed_bump_module/src/manager.hpp index 480208067d87e..40fcfdd081c2e 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class SpeedBumpModuleManager : public SceneModuleManagerInterface private: SpeedBumpModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class SpeedBumpModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_speed_bump_module/src/scene.hpp index 34de722055d3d..f227366127046 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.hpp @@ -26,7 +26,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class SpeedBumpModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_speed_bump_module/src/util.hpp b/planning/behavior_velocity_speed_bump_module/src/util.hpp index fd114772bda60..2cd408735fd61 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.hpp @@ -30,8 +30,8 @@ #include #include -#include #include +#include #include @@ -40,9 +40,9 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point32; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; // the status of intersection between path and speed bump struct PathPolygonIntersectionStatus diff --git a/planning/behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_stop_line_module/CMakeLists.txt index 3c8cc6a4f9626..bef98aafe6f75 100644 --- a/planning/behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/behavior_velocity_stop_line_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_stop_line_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_stop_line_module/package.xml index ee79d64312161..c71ecb83098fb 100644 --- a/planning/behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_stop_line_module/package.xml @@ -18,7 +18,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index 5c9f0fd39c644..b37b3b119178c 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -43,8 +43,7 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { std::vector stop_lines_with_lane_id; @@ -66,8 +65,7 @@ std::vector StopLineModuleManager::getStopLinesWithLaneIdOnP } std::set StopLineModuleManager::getStopLineIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { std::set stop_line_id_set; @@ -78,8 +76,7 @@ std::set StopLineModuleManager::getStopLineIdSetOnPath( return stop_line_id_set; } -void StopLineModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { @@ -95,7 +92,7 @@ void StopLineModuleManager::launchNewModules( std::function &)> StopLineModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto stop_line_id_set = getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); diff --git a/planning/behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_stop_line_module/src/manager.hpp index 4b665a3a536f7..af9dbaa083c33 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -45,17 +45,17 @@ class StopLineModuleManager : public SceneModuleManagerInterface StopLineModule::PlannerParam planner_param_; std::vector getStopLinesWithLaneIdOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); std::set getStopLineIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class StopLineModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_template_module/plugins.xml b/planning/behavior_velocity_template_module/plugins.xml deleted file mode 100644 index 3560c84e6a080..0000000000000 --- a/planning/behavior_velocity_template_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt index cf9392fa0568f..5737802408996 100644 --- a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_traffic_light_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index 544e14f130a7e..ff7b5a269db32 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -19,8 +19,8 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_planning_msgs autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index edddef5cef4d8..01c3acf84cd63 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,17 +41,16 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); - pub_tl_state_ = node.create_publisher( + pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } -void TrafficLightModuleManager::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path) +void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) { visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; - autoware_perception_msgs::msg::TrafficSignal tl_state; + autoware_perception_msgs::msg::TrafficLightGroup tl_state; autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; velocity_factor_array.header.frame_id = "map"; @@ -110,7 +109,7 @@ void TrafficLightModuleManager::modifyPathVelocity( } void TrafficLightModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & traffic_light_reg_elem : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -140,7 +139,7 @@ void TrafficLightModuleManager::launchNewModules( std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 959ef2f91d36c..97340f8592a7d 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -40,12 +40,12 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC private: TrafficLightModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; - void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; + void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; bool isModuleRegisteredFromRegElement(const lanelet::Id & id, const size_t module_id) const; @@ -56,7 +56,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC const lanelet::TrafficLightConstPtr registered_element) const; // Debug - rclcpp::Publisher::SharedPtr pub_tl_state_; + rclcpp::Publisher::SharedPtr pub_tl_state_; std::optional nearest_ref_stop_path_point_index_; }; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 0158251bb42b4..c886578dc65e4 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -75,7 +75,7 @@ std::optional findNearestCollisionPoint( } bool createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, + const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, const double offset, size_t & target_point_idx, Eigen::Vector2d & target_point) { if (input.points.size() < 2) { @@ -140,7 +140,7 @@ bool createTargetPoint( } bool calcStopPointAndInsertIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, const double & stop_line_extend_length, Eigen::Vector2d & stop_line_point, size_t & stop_line_point_idx) @@ -394,12 +394,11 @@ bool TrafficLightModule::isTrafficSignalTimedOut() const return false; } -autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, - const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, - tier4_planning_msgs::msg::StopReason * stop_reason) +tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( + const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, + const Eigen::Vector2d & target_point, tier4_planning_msgs::msg::StopReason * stop_reason) { - autoware_auto_planning_msgs::msg::PathWithLaneId modified_path; + tier4_planning_msgs::msg::PathWithLaneId modified_path; modified_path = input; // Create stop pose diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index d6a409ca7034a..8385a1210d421 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -36,8 +36,8 @@ namespace behavior_velocity_planner class TrafficLightModule : public SceneModuleInterface { public: - using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; using Time = rclcpp::Time; enum class State { APPROACH, GO_OUT }; @@ -45,7 +45,8 @@ class TrafficLightModule : public SceneModuleInterface { double base_link2front; std::vector, autoware_perception_msgs::msg::TrafficSignal>> + std::shared_ptr, + autoware_perception_msgs::msg::TrafficLightGroup>> tl_state; std::vector stop_poses; geometry_msgs::msg::Pose first_stop_pose; @@ -87,10 +88,9 @@ class TrafficLightModule : public SceneModuleInterface private: bool isStopSignal(); - autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, - const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, - tier4_planning_msgs::msg::StopReason * stop_reason); + tier4_planning_msgs::msg::PathWithLaneId insertStopPose( + const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, + const Eigen::Vector2d & target_point, tier4_planning_msgs::msg::StopReason * stop_reason); bool isPassthrough(const double & signed_arc_length) const; diff --git a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml deleted file mode 100644 index 943ef175aa8f8..0000000000000 --- a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_velocity_walkway_module/CMakeLists.txt b/planning/behavior_velocity_walkway_module/CMakeLists.txt index cd41dec991bc6..351a240743402 100644 --- a/planning/behavior_velocity_walkway_module/CMakeLists.txt +++ b/planning/behavior_velocity_walkway_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_walkway_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_walkway_module/package.xml index 2c72959d6b1d4..ea6c7803717f9 100644 --- a/planning/behavior_velocity_walkway_module/package.xml +++ b/planning/behavior_velocity_walkway_module/package.xml @@ -17,7 +17,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_crosswalk_module behavior_velocity_planner_common geometry_msgs diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_walkway_module/src/manager.hpp index 453fcdb40f0db..5228445bb4e03 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_walkway_module/src/manager.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class WalkwayModuleManager : public SceneModuleManagerInterface { diff --git a/planning/costmap_generator/README.md b/planning/costmap_generator/README.md index d30d8b5b58706..cb298195a82a1 100644 --- a/planning/costmap_generator/README.md +++ b/planning/costmap_generator/README.md @@ -6,12 +6,12 @@ This node reads `PointCloud` and/or `DynamicObjectArray` and creates an `Occupan ### Input topics -| Name | Type | Description | -| ------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------- | -| `~input/objects` | autoware_auto_perception_msgs::PredictedObjects | predicted objects, for obstacles areas | -| `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects | -| `~input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map, for drivable areas | -| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | +| Name | Type | Description | +| ------------------------- | ------------------------------------------ | ---------------------------------------------------------------------------- | +| `~input/objects` | autoware_perception_msgs::PredictedObjects | predicted objects, for obstacles areas | +| `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects | +| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map, for drivable areas | +| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | ### Output topics diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp index 87fded0c01891..31c8a05cc3c60 100644 --- a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp +++ b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp @@ -53,8 +53,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -84,7 +84,7 @@ class CostmapGenerator : public rclcpp::Node bool use_parkinglot_; lanelet::LaneletMapPtr lanelet_map_; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_; sensor_msgs::msg::PointCloud2::ConstSharedPtr points_; std::string costmap_frame_; @@ -114,9 +114,8 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_occupancy_grid_; rclcpp::Subscription::SharedPtr sub_points_; - rclcpp::Subscription::SharedPtr - sub_objects_; - rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; + rclcpp::Subscription::SharedPtr sub_objects_; + rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; rclcpp::Subscription::SharedPtr sub_scenario_; rclcpp::TimerBase::SharedPtr timer_; @@ -143,12 +142,12 @@ class CostmapGenerator : public rclcpp::Node void initLaneletMap(); /// \brief callback for loading lanelet2 map - void onLaneletMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onLaneletMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); /// \brief callback for DynamicObjectArray /// \param[in] in_objects input DynamicObjectArray usually from prediction or perception /// component - void onObjects(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + void onObjects(const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); /// \brief callback for sensor_msgs::PointCloud2 /// \param[in] in_points input sensor_msgs::PointCloud2. Assuming ground-filtered pointcloud @@ -190,7 +189,7 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost from DynamicObjectArray /// \param[in] in_objects: subscribed DynamicObjectArray grid_map::Matrix generateObjectsCostmap( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); /// \brief calculate cost from lanelet2 map grid_map::Matrix generatePrimitivesCostmap(); diff --git a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp index 84a7807e432cd..a88bb97a623e2 100644 --- a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp +++ b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp @@ -53,7 +53,7 @@ #include #endif -#include +#include #include @@ -71,7 +71,7 @@ class ObjectsToCostmap grid_map::Matrix makeCostmapFromObjects( const grid_map::GridMap & costmap, const double expand_polygon_size, const double size_of_expansion_kernel, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); private: const int NUMBER_OF_POINTS; @@ -84,7 +84,7 @@ class ObjectsToCostmap /// \param[in] expand_rectangle_size: expanding 4 points /// \param[out] 4 rectangle points Eigen::MatrixXd makeRectanglePoints( - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size); /// \brief make polygon(grid_map::Polygon) from 4 rectangle's points @@ -93,7 +93,7 @@ class ObjectsToCostmap /// \param[out] polygon with 4 rectangle points grid_map::Polygon makePolygonFromObjectBox( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size); /// \brief make expanded point from convex hull's point @@ -111,7 +111,7 @@ class ObjectsToCostmap /// \param[out] polygon object with convex hull points grid_map::Polygon makePolygonFromObjectConvexHull( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_polygon_size); /// \brief set cost in polygon by using DynamicObject's score diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index 8200db93ae2fb..777dfc7a50a6e 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -196,12 +196,12 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) // Subscribers using std::placeholders::_1; - sub_objects_ = this->create_subscription( + sub_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&CostmapGenerator::onObjects, this, _1)); sub_points_ = this->create_subscription( "~/input/points_no_ground", rclcpp::SensorDataQoS(), std::bind(&CostmapGenerator::onPoints, this, _1)); - sub_lanelet_bin_map_ = this->create_subscription( + sub_lanelet_bin_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&CostmapGenerator::onLaneletMapBin, this, _1)); sub_scenario_ = this->create_subscription( @@ -263,7 +263,7 @@ void CostmapGenerator::loadParkingAreasFromLaneletMap( } void CostmapGenerator::onLaneletMapBin( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_); @@ -278,7 +278,7 @@ void CostmapGenerator::onLaneletMapBin( } void CostmapGenerator::onObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { objects_ = msg; } @@ -388,12 +388,12 @@ grid_map::Matrix CostmapGenerator::generatePointsCostmap( return points_costmap; } -autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects( +autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects( const tf2_ros::Buffer & tf_buffer, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects, const std::string & target_frame_id, const std::string & src_frame_id) { - auto objects = new autoware_auto_perception_msgs::msg::PredictedObjects(); + auto objects = new autoware_perception_msgs::msg::PredictedObjects(); *objects = *in_objects; objects->header.frame_id = target_frame_id; @@ -411,11 +411,11 @@ autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformOb object.kinematics.initial_pose_with_covariance.pose = output_stamped.pose; } - return autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr(objects); + return autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr(objects); } grid_map::Matrix CostmapGenerator::generateObjectsCostmap( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) { const auto object_frame = in_objects->header.frame_id; const auto transformed_objects = diff --git a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp b/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp index 79ab4a3bf66c1..f6f024fb92a4e 100644 --- a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp @@ -59,7 +59,7 @@ ObjectsToCostmap::ObjectsToCostmap() } Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints( - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size) { double length = in_object.shape.dimensions.x + expand_rectangle_size; @@ -85,7 +85,7 @@ Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints( grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectBox( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size) { grid_map::Polygon polygon; @@ -122,7 +122,7 @@ geometry_msgs::msg::Point ObjectsToCostmap::makeExpandedPoint( grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectConvexHull( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_polygon_size) { grid_map::Polygon polygon; @@ -157,7 +157,7 @@ void ObjectsToCostmap::setCostInPolygon( grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( const grid_map::GridMap & costmap, const double expand_polygon_size, const double size_of_expansion_kernel, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) { grid_map::GridMap objects_costmap = costmap; objects_costmap.add(OBJECTS_COSTMAP_LAYER_, 0); @@ -165,11 +165,11 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( for (const auto & object : in_objects->objects) { grid_map::Polygon polygon; - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { polygon = makePolygonFromObjectConvexHull(in_objects->header, object, expand_polygon_size); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // TODO(Kenji Miyake): Add makePolygonFromObjectCylinder polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); } diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml index ae124a1774881..dc94d74dce8ae 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/costmap_generator/package.xml @@ -15,8 +15,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_perception_msgs + autoware_map_msgs + autoware_perception_msgs grid_map_ros lanelet2_extension libpcl-all-dev diff --git a/planning/costmap_generator/test/test_objects_to_costmap.cpp b/planning/costmap_generator/test/test_objects_to_costmap.cpp index 667ee70ef53ff..15b853d0782aa 100644 --- a/planning/costmap_generator/test/test_objects_to_costmap.cpp +++ b/planning/costmap_generator/test/test_objects_to_costmap.cpp @@ -17,7 +17,7 @@ #include -using LABEL = autoware_auto_perception_msgs::msg::ObjectClassification; +using LABEL = autoware_perception_msgs::msg::ObjectClassification; class ObjectsToCostMapTest : public ::testing::Test { @@ -69,10 +69,10 @@ grid_y */ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) { - auto objs = std::make_shared(); - autoware_auto_perception_msgs::msg::PredictedObject object; + auto objs = std::make_shared(); + autoware_perception_msgs::msg::PredictedObject object; - object.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); + object.classification.push_back(autoware_perception_msgs::msg::ObjectClassification{}); object.classification.at(0).label = LABEL::CAR; object.classification.at(0).probability = 0.8; @@ -85,7 +85,7 @@ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) object.kinematics.initial_pose_with_covariance.pose.orientation.z = 0; object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1; - object.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; object.shape.dimensions.x = 5; object.shape.dimensions.y = 3; object.shape.dimensions.z = 2; diff --git a/planning/freespace_planner/README.md b/planning/freespace_planner/README.md index b40f2681dce51..6ed0ab605ea05 100644 --- a/planning/freespace_planner/README.md +++ b/planning/freespace_planner/README.md @@ -13,19 +13,19 @@ In other words, the output trajectory doesn't include both forward and backward ### Input topics -| Name | Type | Description | -| ----------------------- | ---------------------------------- | --------------------------------------------------------- | -| `~input/route` | autoware_auto_planning_msgs::Route | route and goal pose | -| `~input/occupancy_grid` | nav_msgs::OccupancyGrid | costmap, for drivable areas | -| `~input/odometry` | nav_msgs::Odometry | vehicle velocity, for checking whether vehicle is stopped | -| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | +| Name | Type | Description | +| ----------------------- | ----------------------------- | --------------------------------------------------------- | +| `~input/route` | autoware_planning_msgs::Route | route and goal pose | +| `~input/occupancy_grid` | nav_msgs::OccupancyGrid | costmap, for drivable areas | +| `~input/odometry` | nav_msgs::Odometry | vehicle velocity, for checking whether vehicle is stopped | +| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | ### Output topics -| Name | Type | Description | -| -------------------- | --------------------------------------- | ------------------------------------------ | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | -| `is_completed` | bool (implemented as rosparam) | whether all split trajectory are published | +| Name | Type | Description | +| -------------------- | ---------------------------------- | ------------------------------------------ | +| `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | +| `is_completed` | bool (implemented as rosparam) | whether all split trajectory are published | ### Output TFs diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp index 2c10b1b491ace..25f3bca215063 100644 --- a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp +++ b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp @@ -38,8 +38,8 @@ #include #include -#include #include +#include #include #include #include @@ -65,8 +65,8 @@ namespace freespace_planner { -using autoware_auto_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Trajectory; using freespace_planning_algorithms::AbstractPlanningAlgorithm; using freespace_planning_algorithms::AstarParam; using freespace_planning_algorithms::AstarSearch; diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index 8ebb22fdb5c2f..8445bd2355b25 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -15,7 +15,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager freespace_planning_algorithms geometry_msgs diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 1c2b4513e9c45..b0d9e9cf3edcd 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -44,9 +44,9 @@ namespace { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; using freespace_planning_algorithms::AstarSearch; using freespace_planning_algorithms::PlannerWaypoint; using freespace_planning_algorithms::PlannerWaypoints; diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index 2fb3e066f0bdd..9b19d8a81bb51 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -19,6 +19,7 @@ geometry_msgs nav_msgs nlohmann-json-dev + pybind11_vendor rosbag2_cpp std_msgs tf2 diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index a9eb488c71377..2f912e30b8246 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -47,10 +47,10 @@ It distributes route requests and planning results according to current MRM oper ### Subscriptions -| Name | Type | Description | -| ----------------------- | ------------------------------------ | ---------------------- | -| `~/input/vector_map` | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2 | -| `~/input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | +| Name | Type | Description | +| --------------------- | ----------------------------------- | ---------------------- | +| `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | +| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | ### Publications @@ -70,7 +70,7 @@ It distributes route requests and planning results according to current MRM oper ![route_sections](./media/route_sections.svg) Route section, whose type is `autoware_planning_msgs/LaneletSegment`, is a "slice" of a road that bundles lane changeable lanes. -Note that the most atomic unit of route is `autoware_auto_mapping_msgs/LaneletPrimitive`, which has the unique id of a lane in a vector map and its type. +Note that the most atomic unit of route is `autoware_planning_msgs/LaneletPrimitive`, which has the unique id of a lane in a vector map and its type. Therefore, route message does not contain geometric information about the lane since we did not want to have planning module’s message to have dependency on map data structure. The ROS message of route section contains following three elements for each route section. diff --git a/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp b/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp index 96de30ddab592..80e197e2a8ef9 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -32,12 +32,12 @@ class PlannerPlugin public: using RoutePoints = std::vector; using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using MarkerArray = visualization_msgs::msg::MarkerArray; virtual ~PlannerPlugin() = default; virtual void initialize(rclcpp::Node * node) = 0; - virtual void initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) = 0; + virtual void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) = 0; virtual bool ready() const = 0; virtual LaneletRoute plan(const RoutePoints & points) = 0; virtual MarkerArray visualize(const LaneletRoute & route) const = 0; diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index b8ad634c9a0d4..f878953d50420 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -18,7 +18,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_mapping_msgs + autoware_map_msgs autoware_planning_msgs geometry_msgs lanelet2_extension diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index d4f12316052d9..5ba152af6611d 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -163,12 +163,12 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) void DefaultPlanner::initialize(rclcpp::Node * node) { initialize_common(node); - map_subscriber_ = node_->create_subscription( + map_subscriber_ = node_->create_subscription( "~/input/vector_map", rclcpp::QoS{10}.transient_local(), std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1)); } -void DefaultPlanner::initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) +void DefaultPlanner::initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) { initialize_common(node); map_callback(msg); @@ -179,7 +179,7 @@ bool DefaultPlanner::ready() const return is_graph_ready_; } -void DefaultPlanner::map_callback(const HADMapBin::ConstSharedPtr msg) +void DefaultPlanner::map_callback(const LaneletMapBin::ConstSharedPtr msg) { route_handler_.setMap(*msg); is_graph_ready_ = true; diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 8c0fbf3b33955..60c232162991d 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -45,7 +45,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin { public: void initialize(rclcpp::Node * node) override; - void initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) override; + void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) override; bool ready() const override; LaneletRoute plan(const RoutePoints & points) override; void updateRoute(const PlannerPlugin::LaneletRoute & route) override; @@ -63,11 +63,11 @@ class DefaultPlanner : public mission_planner::PlannerPlugin DefaultPlannerParameters param_; rclcpp::Node * node_; - rclcpp::Subscription::SharedPtr map_subscriber_; + rclcpp::Subscription::SharedPtr map_subscriber_; rclcpp::Publisher::SharedPtr pub_goal_footprint_marker_; void initialize_common(rclcpp::Node * node); - void map_callback(const HADMapBin::ConstSharedPtr msg); + void map_callback(const LaneletMapBin::ConstSharedPtr msg); /** * @brief check if the goal_footprint is within the combined lanelet of route_lanelets plus the diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index af363bcf23da7..d95bcb8caefae 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -58,7 +58,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) const auto durable_qos = rclcpp::QoS(1).transient_local(); sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1)); - sub_vector_map_ = create_subscription( + sub_vector_map_ = create_subscription( "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1)); sub_reroute_availability_ = create_subscription( "~/input/reroute_availability", rclcpp::QoS(1), @@ -141,7 +141,7 @@ void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSha reroute_availability_ = msg; } -void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg) +void MissionPlanner::on_map(const LaneletMapBin::ConstSharedPtr msg) { map_ptr_ = msg; lanelet_map_ptr_ = std::make_shared(); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 6752ceb4eaa8b..3d712a351d94a 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -44,7 +44,7 @@ namespace mission_planner { -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; @@ -84,19 +84,19 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_reroute_availability_; rclcpp::Publisher::SharedPtr pub_marker_; Odometry::ConstSharedPtr odometry_; - HADMapBin::ConstSharedPtr map_ptr_; + LaneletMapBin::ConstSharedPtr map_ptr_; RerouteAvailability::ConstSharedPtr reroute_availability_; RouteState state_; LaneletRoute::ConstSharedPtr current_route_; lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr}; void on_odometry(const Odometry::ConstSharedPtr msg); - void on_map(const HADMapBin::ConstSharedPtr msg); + void on_map(const LaneletMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt new file mode 100644 index 0000000000000..b96ca3017a031 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_out_of_lane_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_filter_predicted_objects.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md new file mode 100644 index 0000000000000..99396eb0edf22 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md @@ -0,0 +1,165 @@ +## Out of Lane + +### Role + +`out_of_lane` is the module that decelerates and stops to prevent the ego vehicle from entering another lane with incoming dynamic objects. + +### Activation Timing + +This module is activated if `launch_out_of_lane` is set to true. + +### Inner-workings / Algorithms + +The algorithm is made of the following steps. + +1. Calculate the ego path footprints (red). +2. Calculate the other lanes (magenta). +3. Calculate the overlapping ranges between the ego path footprints and the other lanes (green). +4. For each overlapping range, decide if a stop or slow down action must be taken. +5. For each action, insert the corresponding stop or slow down point in the path. + +![overview](./docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png) + +#### 1. Ego Path Footprints + +In this first step, the ego footprint is projected at each path point and are eventually inflated based on the `extra_..._offset` parameters. + +#### 2. Other lanes + +In the second step, the set of lanes to consider for overlaps is generated. +This set is built by selecting all lanelets within some distance from the ego vehicle, and then removing non-relevant lanelets. +The selection distance is chosen as the maximum between the `slowdown.distance_threshold` and the `stop.distance_threshold`. + +A lanelet is deemed non-relevant if it meets one of the following conditions. + +- It is part of the lanelets followed by the ego path. +- It contains the rear point of the ego footprint. +- It follows one of the ego path lanelets. + +#### 3. Overlapping ranges + +In the third step, overlaps between the ego path footprints and the other lanes are calculated. +For each pair of other lane $l$ and ego path footprint $f$, we calculate the overlapping polygons using `boost::geometry::intersection`. +For each overlapping polygon found, if the distance inside the other lane $l$ is above the `overlap.minimum_distance` threshold, then the overlap is ignored. +Otherwise, the arc length range (relative to the ego path) and corresponding points of the overlapping polygons are stored. +Ultimately, for each other lane $l$, overlapping ranges of successive overlaps are built with the following information: + +- overlapped other lane $l$. +- start and end ego path indexes. +- start and end ego path arc lengths. +- start and end overlap points. + +#### 4. Decisions + +In the fourth step, a decision to either slow down or stop before each overlapping range is taken based on the dynamic objects. +The conditions for the decision depend on the value of the `mode` parameter. + +Whether it is decided to slow down or stop is determined by the distance between the ego vehicle and the start of the overlapping range (in arc length along the ego path). +If this distance is bellow the `actions.slowdown.threshold`, a velocity of `actions.slowdown.velocity` will be used. +If the distance is bellow the `actions.stop.threshold`, a velocity of `0`m/s will be used. + + + + + + +
+ + + +
+ +##### Threshold + +With the `mode` set to `"threshold"`, +a decision to stop or slow down before a range is made if +an incoming dynamic object is estimated to reach the overlap within `threshold.time_threshold`. + +##### TTC (time to collision) + +With the `mode` set to `"ttc"`, +estimates for the times when ego and the dynamic objects reach the start and end of the overlapping range are calculated. +This is then used to calculate the time to collision over the period where ego crosses the overlap. +If the time to collision is predicted to go bellow the `ttc.threshold`, the decision to stop or slow down is made. + +##### Intervals + +With the `mode` set to `"intervals"`, +the estimated times when ego and the dynamic objects reach the start and end points of +the overlapping range are used to create time intervals. +These intervals can be made shorter or longer using the +`intervals.ego_time_buffer` and `intervals.objects_time_buffer` parameters. +If the time interval of ego overlaps with the time interval of an object, the decision to stop or slow down is made. + +##### Time estimates + +###### Ego + +To estimate the times when ego will reach an overlap, it is assumed that ego travels along its path +at its current velocity or at half the velocity of the path points, whichever is higher. + +###### Dynamic objects + +Two methods are used to estimate the time when a dynamic objects with reach some point. +If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter. +Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity. + +#### 5. Path update + +Finally, for each decision to stop or slow down before an overlapping range, +a point is inserted in the path. +For a decision taken for an overlapping range with a lane $l$ starting at ego path point index $i$, +a point is inserted in the path between index $i$ and $i-1$ such that the ego footprint projected at the inserted point does not overlap $l$. +Such point with no overlap must exist since, by definition of the overlapping range, +we know that there is no overlap at $i-1$. + +If the point would cause a higher deceleration than allowed by the `max_accel` parameter (node parameter), +it is skipped. + +Moreover, parameter `action.distance_buffer` adds an extra distance between the ego footprint and the overlap when possible. + +### Module Parameters + +| Parameter | Type | Description | +| ----------------------------- | ------ | --------------------------------------------------------------------------------- | +| `mode` | string | [-] mode used to consider a dynamic object. Candidates: threshold, intervals, ttc | +| `skip_if_already_overlapping` | bool | [-] if true, do not run this module when ego already overlaps another lane | + +| Parameter /threshold | Type | Description | +| -------------------- | ------ | ---------------------------------------------------------------- | +| `time_threshold` | double | [s] consider objects that will reach an overlap within this time | + +| Parameter /intervals | Type | Description | +| --------------------- | ------ | ------------------------------------------------------- | +| `ego_time_buffer` | double | [s] extend the ego time interval by this buffer | +| `objects_time_buffer` | double | [s] extend the time intervals of objects by this buffer | + +| Parameter /ttc | Type | Description | +| -------------- | ------ | ------------------------------------------------------------------------------------------------------ | +| `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | + +| Parameter /objects | Type | Description | +| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | +| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | +| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | + +| Parameter /overlap | Type | Description | +| ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | +| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | +| `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | + +| Parameter /action | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | +| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | +| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | +| `slowdown.velocity` | double | [m] slow down velocity | +| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | + +| Parameter /ego | Type | Description | +| -------------------- | ------ | ---------------------------------------------------- | +| `extra_front_offset` | double | [m] extra front distance to add to the ego footprint | +| `extra_rear_offset` | double | [m] extra rear distance to add to the ego footprint | +| `extra_left_offset` | double | [m] extra left distance to add to the ego footprint | +| `extra_right_offset` | double | [m] extra right distance to add to the ego footprint | diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml new file mode 100644 index 0000000000000..e57b5a45d8be6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -0,0 +1,43 @@ +/**: + ros__parameters: + out_of_lane: # module to stop or slowdown before overlapping another lane with other objects + mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc" + skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane + + threshold: + time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time + intervals: # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego + ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer + objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer + ttc: + threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap + + objects: + minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored + use_predicted_paths: true # if true, use the predicted paths to estimate future positions. + # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. + predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. + distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane + cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights + + overlap: + minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered + extra_length: 0.0 # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) + + action: # action to insert in the trajectory if an object causes a conflict at an overlap + skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed + precision: 0.1 # [m] precision when inserting a stop pose in the trajectory + distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled + slowdown: + distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap + velocity: 2.0 # [m/s] slowdown velocity + stop: + distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap + + ego: + min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego + extra_front_offset: 0.0 # [m] extra front distance + extra_rear_offset: 0.0 # [m] extra rear distance + extra_right_offset: 0.0 # [m] extra right distance + extra_left_offset: 0.0 # [m] extra left distance diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png new file mode 100644 index 0000000000000..f095467b3b0ac Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png new file mode 100644 index 0000000000000..2f358975b9491 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png new file mode 100644 index 0000000000000..fdb75ac0c6eb8 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml new file mode 100644 index 0000000000000..051415e9856ed --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -0,0 +1,41 @@ + + + + autoware_motion_velocity_out_of_lane_module + 0.1.0 + The motion_velocity_out_of_lane_module package + + Maxime Clement + Tomoya Kimura + Shumpei Wakabayashi + Takayuki Murooka + + Apache License 2.0 + + autoware_cmake + + autoware_motion_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + traffic_light_utils + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml new file mode 100644 index 0000000000000..0a05163e8f1d0 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp new file mode 100644 index 0000000000000..157d545dcac0d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -0,0 +1,85 @@ +// 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 "calculate_slowdown_points.hpp" + +#include "footprint.hpp" + +#include +#include + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +bool can_decelerate( + const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel) +{ + // TODO(Maxime): use the library function + const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.pose.position, point.pose.position); + const auto acc_to_target_vel = + (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); + return acc_to_target_vel < std::abs(ego_data.max_decel); +} + +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, + const tier4_autoware_utils::Polygon2d & footprint, + const std::optional & prev_slowdown_point, const PlannerParam & params) +{ + const auto from_arc_length = + motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0, ego_data.first_trajectory_idx); + const auto to_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0, decision.target_trajectory_idx); + TrajectoryPoint interpolated_point; + for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { + // TODO(Maxime): binary search + interpolated_point.pose = motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); + const auto respect_decel_limit = + !params.skip_if_over_max_decel || prev_slowdown_point || + can_decelerate(ego_data, interpolated_point, decision.velocity); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.pose); + const auto is_overlap_lane = boost::geometry::overlaps( + interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon()); + const auto is_overlap_extra_lane = + prev_slowdown_point && + boost::geometry::overlaps( + interpolated_footprint, + prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon()); + if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane) + return interpolated_point; + } + return std::nullopt; +} + +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, + const std::optional prev_slowdown_point, PlannerParam params) +{ + params.extra_front_offset += params.dist_buffer; + const auto base_footprint = make_base_footprint(params); + + // search for the first slowdown decision for which a stop point can be inserted + for (const auto & decision : decisions) { + const auto last_in_lane_pose = + calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params); + if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + } + return std::nullopt; +} +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp new file mode 100644 index 0000000000000..19ed066548d0f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -0,0 +1,58 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CALCULATE_SLOWDOWN_POINTS_HPP_ +#define CALCULATE_SLOWDOWN_POINTS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +/// @brief estimate whether ego can decelerate without breaking the deceleration limit +/// @details assume ego wants to reach the target point at the target velocity +/// @param [in] ego_data ego data +/// @param [in] point target point +/// @param [in] target_vel target_velocity +bool can_decelerate( + const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel); + +/// @brief calculate the last pose along the trajectory where ego does not overlap the lane to avoid +/// @param [in] ego_data ego data +/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed) +/// @param [in] footprint the ego footprint +/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits +/// @param [in] params parameters +/// @return the last pose that is not out of lane (if found) +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, + const tier4_autoware_utils::Polygon2d & footprint, + const std::optional & prev_slowdown_point, const PlannerParam & params); + +/// @brief calculate the slowdown point to insert in the trajectory +/// @param ego_data ego data (trajectory, velocity, etc) +/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) +/// @param prev_slowdown_point previously calculated slowdown point +/// @param params parameters +/// @return optional slowdown point to insert in the trajectory +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, + const std::optional prev_slowdown_point, PlannerParam params); +} // namespace autoware::motion_velocity_planner::out_of_lane +#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp new file mode 100644 index 0000000000000..efb957ea85691 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -0,0 +1,242 @@ +// 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 "debug.hpp" + +#include "types.hpp" + +#include + +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane::debug +{ +namespace +{ + +visualization_msgs::msg::Marker get_base_marker() +{ + visualization_msgs::msg::Marker base_marker; + base_marker.header.frame_id = "map"; + base_marker.header.stamp = rclcpp::Time(0); + base_marker.id = 0; + base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + base_marker.action = visualization_msgs::msg::Marker::ADD; + base_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, 0); + base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); + base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + return base_marker; +} +void add_footprint_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, + const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "footprints"; + for (const auto & f : footprints) { + debug_marker.points.clear(); + for (const auto & p : f) + debug_marker.points.push_back( + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + debug_marker.points.clear(); + } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_current_overlap_marker( + visualization_msgs::msg::MarkerArray & debug_marker_array, + const lanelet::BasicPolygon2d & current_footprint, + const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "current_overlap"; + debug_marker.points.clear(); + for (const auto & p : current_footprint) + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z)); + if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); + if (current_overlapped_lanelets.empty()) + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); + else + debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + for (const auto & ll : current_overlapped_lanelets) { + debug_marker.points.clear(); + for (const auto & p : ll.polygon3d()) + debug_marker.points.push_back( + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_lanelet_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, + const lanelet::ConstLanelets & lanelets, const std::string & ns, + const std_msgs::msg::ColorRGBA & color, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = ns; + debug_marker.color = color; + for (const auto & ll : lanelets) { + debug_marker.points.clear(); + + // add a small z offset to draw above the lanelet map + for (const auto & p : ll.polygon3d()) + debug_marker.points.push_back( + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_range_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const std::vector & trajectory_points, + const size_t first_ego_idx, const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "ranges"; + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); + for (const auto & range : ranges) { + debug_marker.points.clear(); + debug_marker.points.push_back( + trajectory_points[first_ego_idx + range.entering_trajectory_idx].pose.position); + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.entering_point.x(), range.entering_point.y(), z)); + for (const auto & overlap : range.debug.overlaps) { + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); + } + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.exiting_point.x(), range.exiting_point.y(), z)); + debug_marker.points.push_back( + trajectory_points[first_ego_idx + range.exiting_trajectory_idx].pose.position); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_decision_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.action = debug_marker.ADD; + debug_marker.id = 0; + debug_marker.ns = "decisions"; + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); + debug_marker.points.clear(); + for (const auto & range : ranges) { + debug_marker.type = debug_marker.LINE_STRIP; + if (range.debug.decision) { + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.entering_point.x(), range.entering_point.y(), z)); + debug_marker.points.push_back( + range.debug.object->kinematics.initial_pose_with_covariance.pose.position); + } + debug_marker_array.markers.push_back(debug_marker); + debug_marker.points.clear(); + debug_marker.id++; + + debug_marker.type = debug_marker.TEXT_VIEW_FACING; + debug_marker.pose.position.x = range.entering_point.x(); + debug_marker.pose.position.y = range.entering_point.y(); + debug_marker.pose.position.z = z; + std::stringstream ss; + ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time + << "\n"; + if (range.debug.object) { + debug_marker.pose.position.x += + range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x; + debug_marker.pose.position.y += + range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y; + debug_marker.pose.position.x /= 2; + debug_marker.pose.position.y /= 2; + ss << "Obj: " << range.debug.times.object.enter_time << " - " + << range.debug.times.object.exit_time << "\n"; + } + debug_marker.scale.z = 1.0; + debug_marker.text = ss.str(); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) { + debug_marker_array.markers.push_back(debug_marker); + } +} +} // namespace +visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data) +{ + constexpr auto z = 0.0; + visualization_msgs::msg::MarkerArray debug_marker_array; + + debug::add_footprint_markers( + debug_marker_array, debug_data.footprints, z, debug_data.prev_footprints); + debug::add_current_overlap_marker( + debug_marker_array, debug_data.current_footprint, debug_data.current_overlapped_lanelets, z, + debug_data.prev_current_overlapped_lanelets); + debug::add_lanelet_markers( + debug_marker_array, debug_data.trajectory_lanelets, "trajectory_lanelets", + tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), + debug_data.prev_trajectory_lanelets); + debug::add_lanelet_markers( + debug_marker_array, debug_data.ignored_lanelets, "ignored_lanelets", + tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), debug_data.prev_ignored_lanelets); + debug::add_lanelet_markers( + debug_marker_array, debug_data.other_lanelets, "other_lanelets", + tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data.prev_other_lanelets); + debug::add_range_markers( + debug_marker_array, debug_data.ranges, debug_data.trajectory_points, + debug_data.first_trajectory_idx, z, debug_data.prev_ranges); + debug::add_decision_markers(debug_marker_array, debug_data.ranges, z, debug_data.prev_ranges); + return debug_marker_array; +} + +motion_utils::VirtualWalls create_virtual_walls( + const DebugData & debug_data, const PlannerParam & params) +{ + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; + wall.text = "out_of_lane"; + wall.longitudinal_offset = params.front_offset; + wall.style = motion_utils::VirtualWallType::slowdown; + for (const auto & slowdown : debug_data.slowdowns) { + wall.pose = slowdown.point.pose; + virtual_walls.push_back(wall); + } + return virtual_walls; +} +} // namespace autoware::motion_velocity_planner::out_of_lane::debug diff --git a/localization/geo_pose_projector/src/geo_pose_projector_node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp similarity index 50% rename from localization/geo_pose_projector/src/geo_pose_projector_node.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp index 97367d6b9f774..4a5be9dfb07ac 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector_node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Autoware Foundation +// 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. @@ -12,18 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "geo_pose_projector.hpp" +#ifndef DEBUG_HPP_ +#define DEBUG_HPP_ -#include +#include "types.hpp" -#include +#include -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); +#include - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); +namespace autoware::motion_velocity_planner::out_of_lane::debug +{ +visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data); +motion_utils::VirtualWalls create_virtual_walls( + const DebugData & debug_data, const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane::debug - return 0; -} +#endif // DEBUG_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp new file mode 100644 index 0000000000000..81610f750e753 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp @@ -0,0 +1,388 @@ +// 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 "decisions.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +namespace autoware::motion_velocity_planner::out_of_lane +{ +double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx) +{ + return motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.pose.position, ego_data.first_trajectory_idx + target_idx); +} + +double time_along_trajectory( + const EgoData & ego_data, const size_t target_idx, const double min_velocity) +{ + const auto dist = distance_along_trajectory(ego_data, target_idx); + const auto v = std::max( + std::max(ego_data.velocity, min_velocity), + ego_data.trajectory_points[ego_data.first_trajectory_idx + target_idx] + .longitudinal_velocity_mps * + 0.5); + return dist / v; +} + +bool object_is_incoming( + const lanelet::BasicPoint2d & object_position, + const std::shared_ptr route_handler, + const lanelet::ConstLanelet & lane) +{ + const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); + if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; + for (const auto & lls : lanelets) + for (const auto & ll : lls) + if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; + return false; +} + +std::optional> object_time_to_range( + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const std::shared_ptr route_handler, const double dist_buffer, + const rclcpp::Logger & logger) +{ + // skip the dynamic object if it is not in a lane preceding the overlapped lane + // lane changes are intentionally not considered + const auto object_point = lanelet::BasicPoint2d( + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y); + if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; + + const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; + const auto max_lon_deviation = object.shape.dimensions.x / 2.0; + auto worst_enter_time = std::optional(); + auto worst_exit_time = std::optional(); + + for (const auto & predicted_path : object.kinematics.predicted_paths) { + const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); + if (unique_path_points.size() < 2) continue; + const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); + const auto enter_point = + geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); + const auto enter_segment_idx = + motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); + const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment( + unique_path_points, enter_segment_idx, enter_point); + const auto enter_lat_dist = + std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); + const auto enter_segment_length = tier4_autoware_utils::calcDistance2d( + unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); + const auto enter_offset_ratio = enter_offset / enter_segment_length; + const auto enter_time = + static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; + + const auto exit_point = + geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); + const auto exit_segment_idx = + motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); + const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment( + unique_path_points, exit_segment_idx, exit_point); + const auto exit_lat_dist = + std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); + const auto exit_segment_length = tier4_autoware_utils::calcDistance2d( + unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); + const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); + const auto exit_time = + static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; + + RCLCPP_DEBUG( + logger, "\t\t\tPredicted path (time step = %2.2fs): enter @ %2.2fs, exit @ %2.2fs", time_step, + enter_time, exit_time); + // predicted path is too far from the overlapping range to be relevant + const auto is_far_from_entering_point = enter_lat_dist > max_deviation; + const auto is_far_from_exiting_point = exit_lat_dist > max_deviation; + if (is_far_from_entering_point && is_far_from_exiting_point) { + RCLCPP_DEBUG( + logger, + " * far_from_enter (%d) = %2.2fm | far_from_exit (%d) = %2.2fm | max_dev = %2.2fm\n", + is_far_from_entering_point, enter_lat_dist, is_far_from_exiting_point, exit_lat_dist, + max_deviation); + continue; + } + const auto is_last_predicted_path_point = + (exit_segment_idx + 2 == unique_path_points.size() || + enter_segment_idx + 2 == unique_path_points.size()); + const auto does_not_reach_overlap = + is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); + if (does_not_reach_overlap) { + RCLCPP_DEBUG( + logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", + std::min(exit_offset, enter_offset), max_lon_deviation); + continue; + } + + const auto same_driving_direction_as_ego = enter_time < exit_time; + if (same_driving_direction_as_ego) { + worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; + worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; + } else { + worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; + worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; + } + } + if (worst_enter_time && worst_exit_time) { + RCLCPP_DEBUG( + logger, "\t\t\t * found enter/exit time [%2.2f, %2.2f]\n", *worst_enter_time, + *worst_exit_time); + return std::make_pair(*worst_enter_time, *worst_exit_time); + } + RCLCPP_DEBUG(logger, "\t\t\t * enter/exit time not found\n"); + return {}; +} + +std::optional> object_time_to_range( + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const DecisionInputs & inputs, const rclcpp::Logger & logger) +{ + const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; + const auto object_point = lanelet::BasicPoint2d(p.x, p.y); + const auto half_size = object.shape.dimensions.x / 2.0; + lanelet::ConstLanelets object_lanelets; + for (const auto & ll : inputs.lanelets) + if (boost::geometry::within(object_point, ll.polygon2d().basicPolygon())) + object_lanelets.push_back(ll); + + geometry_msgs::msg::Pose pose; + pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); + const auto range_enter_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; + pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); + const auto range_exit_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; + const auto range_size = std::abs(range_enter_length - range_exit_length); + auto worst_enter_dist = std::optional(); + auto worst_exit_dist = std::optional(); + for (const auto & lane : object_lanelets) { + const auto path = inputs.route_handler->getRoutingGraphPtr()->shortestPath(lane, range.lane); + RCLCPP_DEBUG( + logger, "\t\t\tPath ? %d [from %ld to %ld]\n", path.has_value(), lane.id(), range.lane.id()); + if (path) { + lanelet::ConstLanelets lls; + for (const auto & ll : *path) lls.push_back(ll); + pose.position.set__x(object_point.x()).set__y(object_point.y()); + const auto object_curr_length = lanelet::utils::getArcCoordinates(lls, pose).length; + pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); + const auto enter_dist = + lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; + pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); + const auto exit_dist = + lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; + RCLCPP_DEBUG( + logger, "\t\t\t%2.2f -> [%2.2f(%2.2f, %2.2f) - %2.2f(%2.2f, %2.2f)]\n", object_curr_length, + enter_dist, range.entering_point.x(), range.entering_point.y(), exit_dist, + range.exiting_point.x(), range.exiting_point.y()); + const auto already_entered_range = std::abs(enter_dist - exit_dist) > range_size * 2.0; + if (already_entered_range) continue; + // multiple paths to the overlap -> be conservative and use the "worst" case + // "worst" = min/max arc length depending on if the lane is running opposite to the ego + // trajectory + const auto is_opposite = enter_dist > exit_dist; + if (!worst_enter_dist) + worst_enter_dist = enter_dist; + else if (is_opposite) + worst_enter_dist = std::max(*worst_enter_dist, enter_dist); + else + worst_enter_dist = std::min(*worst_enter_dist, enter_dist); + if (!worst_exit_dist) + worst_exit_dist = exit_dist; + else if (is_opposite) + worst_exit_dist = std::max(*worst_exit_dist, exit_dist); + else + worst_exit_dist = std::min(*worst_exit_dist, exit_dist); + } + } + if (worst_enter_dist && worst_exit_dist) { + const auto v = object.kinematics.initial_twist_with_covariance.twist.linear.x; + return std::make_pair((*worst_enter_dist - half_size) / v, (*worst_exit_dist + half_size) / v); + } + return {}; +} + +bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) +{ + const auto enter_within_threshold = + range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; + const auto exit_within_threshold = + range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; + return enter_within_threshold || exit_within_threshold; +} + +bool intervals_condition( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) +{ + const auto opposite_way_condition = [&]() -> bool { + const auto ego_exits_before_object_enters = + range_times.ego.exit_time + params.intervals_ego_buffer < + range_times.object.enter_time + params.intervals_obj_buffer; + RCLCPP_DEBUG( + logger, + "\t\t\t[Intervals] (opposite way) ego exit %2.2fs < obj enter %2.2fs ? -> should not " + "enter = %d\n", + range_times.ego.exit_time + params.intervals_ego_buffer, + range_times.object.enter_time + params.intervals_obj_buffer, ego_exits_before_object_enters); + return ego_exits_before_object_enters; + }; + const auto same_way_condition = [&]() -> bool { + const auto object_enters_during_overlap = + range_times.ego.enter_time - params.intervals_ego_buffer < + range_times.object.enter_time + params.intervals_obj_buffer && + range_times.object.enter_time - params.intervals_obj_buffer - range_times.ego.exit_time < + range_times.ego.exit_time + params.intervals_ego_buffer; + const auto object_exits_during_overlap = + range_times.ego.enter_time - params.intervals_ego_buffer < + range_times.object.exit_time + params.intervals_obj_buffer && + range_times.object.exit_time - params.intervals_obj_buffer - range_times.ego.exit_time < + range_times.ego.exit_time + params.intervals_ego_buffer; + RCLCPP_DEBUG( + logger, + "\t\t\t[Intervals] obj enters during overlap ? %d OR obj exits during overlap %d ? -> should " + "not " + "enter = %d\n", + object_enters_during_overlap, object_exits_during_overlap, + object_enters_during_overlap || object_exits_during_overlap); + return object_enters_during_overlap || object_exits_during_overlap; + }; + + const auto object_is_going_same_way = + range_times.object.enter_time < range_times.object.exit_time; + return (object_is_going_same_way && same_way_condition()) || + (!object_is_going_same_way && opposite_way_condition()); +} + +bool ttc_condition( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) +{ + const auto ttc_at_enter = range_times.ego.enter_time - range_times.object.enter_time; + const auto ttc_at_exit = range_times.ego.exit_time - range_times.object.exit_time; + const auto collision_during_overlap = (ttc_at_enter < 0.0) != (ttc_at_exit < 0.0); + const auto ttc_is_bellow_threshold = + std::min(std::abs(ttc_at_enter), std::abs(ttc_at_exit)) <= params.ttc_threshold; + RCLCPP_DEBUG( + logger, "\t\t\t[TTC] (%2.2fs - %2.2fs) -> %d\n", ttc_at_enter, ttc_at_exit, + (collision_during_overlap || ttc_is_bellow_threshold)); + return collision_during_overlap || ttc_is_bellow_threshold; +} + +bool will_collide_on_range( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) +{ + RCLCPP_DEBUG( + logger, " enter at %2.2fs, exits at %2.2fs\n", range_times.object.enter_time, + range_times.object.exit_time); + return (params.mode == "threshold" && threshold_condition(range_times, params)) || + (params.mode == "intervals" && intervals_condition(range_times, params, logger)) || + (params.mode == "ttc" && ttc_condition(range_times, params, logger)); +} + +bool should_not_enter( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger) +{ + RangeTimes range_times{}; + range_times.ego.enter_time = + time_along_trajectory(inputs.ego_data, range.entering_trajectory_idx, params.ego_min_velocity); + range_times.ego.exit_time = + time_along_trajectory(inputs.ego_data, range.exiting_trajectory_idx, params.ego_min_velocity); + RCLCPP_DEBUG( + logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", + range.entering_trajectory_idx, range.exiting_trajectory_idx, range.lane.id(), + range_times.ego.enter_time, range_times.ego.exit_time); + for (const auto & object : inputs.objects.objects) { + RCLCPP_DEBUG( + logger, "\t\t[%s] going at %2.2fm/s", + tier4_autoware_utils::toHexString(object.object_id).c_str(), + object.kinematics.initial_twist_with_covariance.twist.linear.x); + if (object.kinematics.initial_twist_with_covariance.twist.linear.x < params.objects_min_vel) { + RCLCPP_DEBUG(logger, " SKIP (velocity bellow threshold %2.2fm/s)\n", params.objects_min_vel); + continue; // skip objects with velocity bellow a threshold + } + // skip objects that are already on the interval + const auto enter_exit_time = + params.objects_use_predicted_paths + ? object_time_to_range( + object, range, inputs.route_handler, params.objects_dist_buffer, logger) + : object_time_to_range(object, range, inputs, logger); + if (!enter_exit_time) { + RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); + continue; // skip objects that are not driving towards the overlapping range + } + + range_times.object.enter_time = enter_exit_time->first; + range_times.object.exit_time = enter_exit_time->second; + if (will_collide_on_range(range_times, params, logger)) { + range.debug.times = range_times; + range.debug.object = object; + return true; + } + } + range.debug.times = range_times; + return false; +} + +void set_decision_velocity( + std::optional & decision, const double distance, const PlannerParam & params) +{ + if (distance < params.stop_dist_threshold) { + decision->velocity = 0.0; + } else if (distance < params.slow_dist_threshold) { + decision->velocity = params.slow_velocity; + } else { + decision.reset(); + } +} + +std::optional calculate_decision( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger) +{ + std::optional decision; + if (should_not_enter(range, inputs, params, logger)) { + decision.emplace(); + decision->target_trajectory_idx = inputs.ego_data.first_trajectory_idx + + range.entering_trajectory_idx; // add offset from curr pose + decision->lane_to_avoid = range.lane; + const auto ego_dist_to_range = + distance_along_trajectory(inputs.ego_data, range.entering_trajectory_idx); + set_decision_velocity(decision, ego_dist_to_range, params); + } + return decision; +} + +std::vector calculate_decisions( + const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger) +{ + std::vector decisions; + for (const auto & range : inputs.ranges) { + if (range.entering_trajectory_idx == 0UL) continue; // skip if we already entered the range + const auto optional_decision = calculate_decision(range, inputs, params, logger); + range.debug.decision = optional_decision; + if (optional_decision) decisions.push_back(*optional_decision); + } + return decisions; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp new file mode 100644 index 0000000000000..e691f2893dab9 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp @@ -0,0 +1,116 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DECISIONS_HPP_ +#define DECISIONS_HPP_ + +#include "types.hpp" + +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +/// @brief calculate the distance along the ego trajectory between ego and some target trajectory +/// index +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] target_idx target ego trajectory index +/// @return distance between ego and the target [m] +double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx); +/// @brief estimate the time when ego will reach some target trajectory index +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] target_idx target ego trajectory index +/// @param [in] min_velocity minimum ego velocity used to estimate the time +/// @return time taken by ego to reach the target [s] +double time_along_trajectory(const EgoData & ego_data, const size_t target_idx); +/// @brief use an object's predicted paths to estimate the times it will reach the enter and exit +/// points of an overlapping range +/// @details times when the predicted paths of the object enters/exits the range are calculated +/// but may not exist (e.g,, predicted path ends before reaching the end of the range) +/// @param [in] object dynamic object +/// @param [in] range overlapping range +/// @param [in] route_handler route handler used to estimate the path of the dynamic object +/// @param [in] logger ros logger +/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in +/// the opposite direction, time at enter > time at exit +std::optional> object_time_to_range( + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const std::shared_ptr route_handler, const double dist_buffer, + const rclcpp::Logger & logger); +/// @brief use the lanelet map to estimate the times when an object will reach the enter and exit +/// points of an overlapping range +/// @param [in] object dynamic object +/// @param [in] range overlapping range +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range +/// @param [in] logger ros logger +/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in +/// the opposite direction, time at enter > time at exit. +std::optional> object_time_to_range( + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const DecisionInputs & inputs, const rclcpp::Logger & logger); +/// @brief decide whether an object is coming in the range at the same time as ego +/// @details the condition depends on the mode (threshold, intervals, ttc) +/// @param [in] range_times times when ego and the object enter/exit the range +/// @param [in] params parameters +/// @param [in] logger ros logger +bool will_collide_on_range( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); +/// @brief check whether we should avoid entering the given range +/// @param [in] range the range to check +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] params parameters +/// @param [in] logger ros logger +/// @return return true if we should avoid entering the range +bool should_not_enter( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger); +/// @brief set the velocity of a decision (or unset it) based on the distance away from the range +/// @param [out] decision decision to update (either set its velocity or unset the decision) +/// @param [in] distance distance between ego and the range corresponding to the decision +/// @param [in] params parameters +void set_decision_velocity( + std::optional & decision, const double distance, const PlannerParam & params); +/// @brief calculate the decision to slowdown or stop before an overlapping range +/// @param [in] range the range to check +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] params parameters +/// @param [in] logger ros logger +/// @return return an optional decision to slowdown or stop +std::optional calculate_decision( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger); +/// @brief calculate decisions to slowdown or stop before some overlapping ranges +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] params parameters +/// @param [in] logger ros logger +/// @return return the calculated decisions to slowdown or stop +std::vector calculate_decisions( + const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // DECISIONS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp new file mode 100644 index 0000000000000..ce993dc1cedb0 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -0,0 +1,150 @@ +// 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 "filter_predicted_objects.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +void cut_predicted_path_beyond_line( + autoware_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) +{ + if (predicted_path.path.empty() || stop_line.size() < 2) return; + + auto stop_line_idx = 0UL; + bool found = false; + lanelet::BasicSegment2d path_segment; + path_segment.first.x() = predicted_path.path.front().position.x; + path_segment.first.y() = predicted_path.path.front().position.y; + for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) { + path_segment.second.x() = predicted_path.path[stop_line_idx].position.x; + path_segment.second.y() = predicted_path.path[stop_line_idx].position.y; + if (boost::geometry::intersects(stop_line, path_segment)) { + found = true; + break; + } + path_segment.first = path_segment.second; + } + if (found) { + auto cut_idx = stop_line_idx; + double arc_length = 0; + while (cut_idx > 0 && arc_length < object_front_overhang) { + arc_length += tier4_autoware_utils::calcDistance2d( + predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); + --cut_idx; + } + predicted_path.path.resize(cut_idx); + } +} + +std::optional find_next_stop_line( + const autoware_perception_msgs::msg::PredictedPath & path, + const std::shared_ptr planner_data) +{ + lanelet::ConstLanelets lanelets; + lanelet::BasicLineString2d query_line; + for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y); + const auto query_results = lanelet::geometry::findWithin2d( + planner_data->route_handler->getLaneletMapPtr()->laneletLayer, query_line); + for (const auto & r : query_results) lanelets.push_back(r.second); + for (const auto & ll : lanelets) { + for (const auto & element : ll.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->get_traffic_signal(element->id()); + if ( + traffic_signal_stamped.has_value() && element->stopLine().has_value() && + traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) { + lanelet::BasicLineString2d stop_line; + for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y()); + return stop_line; + } + } + } + return std::nullopt; +} + +void cut_predicted_path_beyond_red_lights( + autoware_perception_msgs::msg::PredictedPath & predicted_path, + const std::shared_ptr planner_data, const double object_front_overhang) +{ + const auto stop_line = find_next_stop_line(predicted_path, planner_data); + if (stop_line) { + // we use a longer stop line to also cut predicted paths that slightly go around the stop line + auto longer_stop_line = *stop_line; + const auto diff = stop_line->back() - stop_line->front(); + longer_stop_line.front() -= diff * 0.5; + longer_stop_line.back() += diff * 0.5; + cut_predicted_path_beyond_line(predicted_path, longer_stop_line, object_front_overhang); + } +} + +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const std::shared_ptr planner_data, const EgoData & ego_data, + const PlannerParam & params) +{ + autoware_perception_msgs::msg::PredictedObjects filtered_objects; + filtered_objects.header = planner_data->predicted_objects->header; + for (const auto & object : planner_data->predicted_objects->objects) { + const auto is_pedestrian = + std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != object.classification.end(); + if (is_pedestrian) continue; + + auto filtered_object = object; + const auto is_invalid_predicted_path = [&](const auto & predicted_path) { + const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; + const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); + if (no_overlap_path.size() <= 1) return true; + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + const auto is_crossing_ego = + lat_offset_to_current_ego <= + object.shape.dimensions.y / 2.0 + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); + return is_low_confidence || is_crossing_ego; + }; + if (params.objects_use_predicted_paths) { + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + if (params.objects_cut_predicted_paths_beyond_red_lights) + for (auto & predicted_path : predicted_paths) + cut_predicted_path_beyond_red_lights( + predicted_path, planner_data, filtered_object.shape.dimensions.x); + predicted_paths.erase( + std::remove_if( + predicted_paths.begin(), predicted_paths.end(), + [](const auto & p) { return p.path.empty(); }), + predicted_paths.end()); + } + + if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + filtered_objects.objects.push_back(filtered_object); + } + return filtered_objects; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp new file mode 100644 index 0000000000000..3083d85df42ab --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -0,0 +1,60 @@ +// Copyright 2024-2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FILTER_PREDICTED_OBJECTS_HPP_ +#define FILTER_PREDICTED_OBJECTS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +/// @brief cut a predicted path beyond the given stop line +/// @param [inout] predicted_path predicted path to cut +/// @param [in] stop_line stop line used for cutting +/// @param [in] object_front_overhang extra distance to cut ahead of the stop line +void cut_predicted_path_beyond_line( + autoware_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); + +/// @brief find the next red light stop line along the given path +/// @param [in] path predicted path to check for a stop line +/// @param [in] planner_data planner data with stop line information +/// @return the first red light stop line found along the path (if any) +std::optional find_next_stop_line( + const autoware_perception_msgs::msg::PredictedPath & path, + const std::shared_ptr planner_data); + +/// @brief cut predicted path beyond stop lines of red lights +/// @param [inout] predicted_path predicted path to cut +/// @param [in] planner_data planner data to get the map and traffic light information +void cut_predicted_path_beyond_red_lights( + autoware_perception_msgs::msg::PredictedPath & predicted_path, + const std::shared_ptr planner_data, const double object_front_overhang); + +/// @brief filter predicted objects and their predicted paths +/// @param [in] planner_data planner data +/// @param [in] ego_data ego data +/// @param [in] params parameters +/// @return filtered predicted objects +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const std::shared_ptr planner_data, const EgoData & ego_data, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp new file mode 100644 index 0000000000000..824a847b17414 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -0,0 +1,93 @@ +// 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 "footprint.hpp" + +#include + +#include + +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +tier4_autoware_utils::Polygon2d make_base_footprint( + const PlannerParam & p, const bool ignore_offset) +{ + tier4_autoware_utils::Polygon2d base_footprint; + const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; + const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; + const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; + const auto left_offset = ignore_offset ? 0.0 : p.extra_left_offset; + base_footprint.outer() = { + {p.front_offset + front_offset, p.left_offset + left_offset}, + {p.front_offset + front_offset, p.right_offset - right_offset}, + {p.rear_offset - rear_offset, p.right_offset - right_offset}, + {p.rear_offset - rear_offset, p.left_offset + left_offset}}; + return base_footprint; +} + +lanelet::BasicPolygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) +{ + const auto angle = tf2::getYaw(pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + lanelet::BasicPolygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); + return footprint; +} + +std::vector calculate_trajectory_footprints( + const EgoData & ego_data, const PlannerParam & params) +{ + const auto base_footprint = make_base_footprint(params); + std::vector trajectory_footprints; + trajectory_footprints.reserve(ego_data.trajectory_points.size()); + double length = 0.0; + const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) + + params.front_offset + params.extra_front_offset; + for (auto i = ego_data.first_trajectory_idx; + i < ego_data.trajectory_points.size() && length < range; ++i) { + const auto & trajectory_pose = ego_data.trajectory_points[i].pose; + const auto angle = tf2::getYaw(trajectory_pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + lanelet::BasicPolygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.emplace_back( + p.x() + trajectory_pose.position.x, p.y() + trajectory_pose.position.y); + trajectory_footprints.push_back(footprint); + if (i + 1 < ego_data.trajectory_points.size()) + length += tier4_autoware_utils::calcDistance2d( + trajectory_pose, ego_data.trajectory_points[i + 1].pose); + } + return trajectory_footprints; +} + +lanelet::BasicPolygon2d calculate_current_ego_footprint( + const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset) +{ + const auto base_footprint = make_base_footprint(params, ignore_offset); + const auto angle = tf2::getYaw(ego_data.pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + lanelet::BasicPolygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); + return footprint; +} +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp new file mode 100644 index 0000000000000..7b80dd9618bb7 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp @@ -0,0 +1,59 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FOOTPRINT_HPP_ +#define FOOTPRINT_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace autoware::motion_velocity_planner +{ +namespace out_of_lane +{ +/// @brief create the base footprint of ego +/// @param [in] p parameters used to create the footprint +/// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the +/// footprint +/// @return base ego footprint +tier4_autoware_utils::Polygon2d make_base_footprint( + const PlannerParam & p, const bool ignore_offset = false); +/// @brief project a footprint to the given pose +/// @param [in] base_footprint footprint to project +/// @param [in] pose projection pose +/// @return footprint projected to the given pose +lanelet::BasicPolygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); +/// @brief calculate the trajectory footprints +/// @details the resulting polygon follows the format used by the lanelet library: clockwise order +/// and implicit closing edge +/// @param [in] ego_data data related to the ego vehicle (includes its trajectory) +/// @param [in] params parameters +/// @return polygon footprints for each trajectory point starting from ego's current position +std::vector calculate_trajectory_footprints( + const EgoData & ego_data, const PlannerParam & params); +/// @brief calculate the current ego footprint +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] params parameters +/// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the +/// footprint +lanelet::BasicPolygon2d calculate_current_ego_footprint( + const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset = false); +} // namespace out_of_lane +} // namespace autoware::motion_velocity_planner + +#endif // FOOTPRINT_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp new file mode 100644 index 0000000000000..23dbf77f08151 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -0,0 +1,126 @@ +// 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 "lanelets_selection.hpp" + +#include + +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +lanelet::ConstLanelets consecutive_lanelets( + const std::shared_ptr route_handler, + const lanelet::ConstLanelet & lanelet) +{ + lanelet::ConstLanelets consecutives = route_handler->getRoutingGraphPtr()->following(lanelet); + const auto previous = route_handler->getRoutingGraphPtr()->previous(lanelet); + consecutives.insert(consecutives.end(), previous.begin(), previous.end()); + return consecutives; +} + +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler) +{ + lanelet::ConstLanelets missing_lane_change_lanelets; + const auto & routing_graph = *route_handler->getRoutingGraphPtr(); + lanelet::ConstLanelets adjacents; + lanelet::ConstLanelets consecutives; + for (const auto & ll : trajectory_lanelets) { + const auto consecutives_of_ll = consecutive_lanelets(route_handler, ll); + std::copy_if( + consecutives_of_ll.begin(), consecutives_of_ll.end(), std::back_inserter(consecutives), + [&](const auto & l) { return !contains_lanelet(consecutives, l.id()); }); + const auto adjacents_of_ll = routing_graph.besides(ll); + std::copy_if( + adjacents_of_ll.begin(), adjacents_of_ll.end(), std::back_inserter(adjacents), + [&](const auto & l) { return !contains_lanelet(adjacents, l.id()); }); + } + std::copy_if( + adjacents.begin(), adjacents.end(), std::back_inserter(missing_lane_change_lanelets), + [&](const auto & l) { + return !contains_lanelet(missing_lane_change_lanelets, l.id()) && + !contains_lanelet(trajectory_lanelets, l.id()) && + contains_lanelet(consecutives, l.id()); + }); + return missing_lane_change_lanelets; +} + +lanelet::ConstLanelets calculate_trajectory_lanelets( + const EgoData & ego_data, const std::shared_ptr route_handler) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + lanelet::ConstLanelets trajectory_lanelets; + lanelet::BasicLineString2d trajectory_ls; + for (const auto & p : ego_data.trajectory_points) + trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); + for (const auto & dist_lanelet : + lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, trajectory_ls)) { + if (!contains_lanelet(trajectory_lanelets, dist_lanelet.second.id())) + trajectory_lanelets.push_back(dist_lanelet.second); + } + const auto missing_lanelets = + get_missing_lane_change_lanelets(trajectory_lanelets, route_handler); + trajectory_lanelets.insert( + trajectory_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end()); + return trajectory_lanelets; +} + +lanelet::ConstLanelets calculate_ignored_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params) +{ + lanelet::ConstLanelets ignored_lanelets; + // ignore lanelets directly behind ego + const auto behind = + tier4_autoware_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); + const lanelet::BasicPoint2d behind_point(behind.position.x, behind.position.y); + const auto behind_lanelets = lanelet::geometry::findWithin2d( + route_handler->getLaneletMapPtr()->laneletLayer, behind_point, 0.0); + for (const auto & l : behind_lanelets) { + const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, l.second.id()); + if (!is_trajectory_lanelet) ignored_lanelets.push_back(l.second); + } + return ignored_lanelets; +} + +lanelet::ConstLanelets calculate_other_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params) +{ + lanelet::ConstLanelets other_lanelets; + const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y); + const auto lanelets_within_range = lanelet::geometry::findWithin2d( + route_handler->getLaneletMapPtr()->laneletLayer, ego_point, + std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset + + params.extra_front_offset); + for (const auto & ll : lanelets_within_range) { + if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") + continue; + const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.second.id()); + const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); + if (!is_trajectory_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); + } + return other_lanelets; +} +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp new file mode 100644 index 0000000000000..c7351f0a98e89 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -0,0 +1,80 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANELETS_SELECTION_HPP_ +#define LANELETS_SELECTION_HPP_ + +#include "types.hpp" + +#include + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +/// @brief checks if a lanelet is already contained in a vector of lanelets +/// @param [in] lanelets vector to check +/// @param [in] id lanelet id to check +/// @return true if the given vector contains a lanelet of the given id +inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lanelet::Id id) +{ + return std::find_if(lanelets.begin(), lanelets.end(), [&](const auto & l) { + return l.id() == id; + }) != lanelets.end(); +}; + +/// @brief calculate lanelets crossed by the ego trajectory +/// @details calculated from the ids of the trajectory msg, the lanelets containing trajectory +/// points +/// @param [in] ego_data data about the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets crossed by the ego vehicle +lanelet::ConstLanelets calculate_trajectory_lanelets( + const EgoData & ego_data, const std::shared_ptr route_handler); +/// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during +/// a lane change +/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets that may be overlapped by a lane change (and are not already in +/// trajectory_lanelets) +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler); +/// @brief calculate lanelets that should be ignored +/// @param [in] ego_data data about the ego vehicle +/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] route_handler route handler +/// @param [in] params parameters +/// @return lanelets to ignore +lanelet::ConstLanelets calculate_ignored_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params); +/// @brief calculate lanelets that should be checked by the module +/// @param [in] ego_data data about the ego vehicle +/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] ignored_lanelets lanelets to ignore +/// @param [in] route_handler route handler +/// @param [in] params parameters +/// @return lanelets to check for overlaps +lanelet::ConstLanelets calculate_other_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // LANELETS_SELECTION_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp new file mode 100644 index 0000000000000..2cd050d15dd9b --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -0,0 +1,305 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "out_of_lane_module.hpp" + +#include "calculate_slowdown_points.hpp" +#include "debug.hpp" +#include "decisions.hpp" +#include "filter_predicted_objects.hpp" +#include "footprint.hpp" +#include "lanelets_selection.hpp" +#include "overlapping_range.hpp" +#include "types.hpp" + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + logger_ = node.get_logger(); + clock_ = node.get_clock(); + init_parameters(node); + velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + debug_publisher_ = + node.create_publisher("~/" + ns_ + "/debug_markers", 1); + virtual_wall_publisher_ = + node.create_publisher("~/" + ns_ + "/virtual_walls", 1); +} +void OutOfLaneModule::init_parameters(rclcpp::Node & node) +{ + using tier4_autoware_utils::getOrDeclareParameter; + auto & pp = params_; + + pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); + pp.skip_if_already_overlapping = + getOrDeclareParameter(node, ns_ + ".skip_if_already_overlapping"); + + pp.time_threshold = getOrDeclareParameter(node, ns_ + ".threshold.time_threshold"); + pp.intervals_ego_buffer = getOrDeclareParameter(node, ns_ + ".intervals.ego_time_buffer"); + pp.intervals_obj_buffer = + getOrDeclareParameter(node, ns_ + ".intervals.objects_time_buffer"); + pp.ttc_threshold = getOrDeclareParameter(node, ns_ + ".ttc.threshold"); + + pp.objects_min_vel = getOrDeclareParameter(node, ns_ + ".objects.minimum_velocity"); + pp.objects_use_predicted_paths = + getOrDeclareParameter(node, ns_ + ".objects.use_predicted_paths"); + pp.objects_min_confidence = + getOrDeclareParameter(node, ns_ + ".objects.predicted_path_min_confidence"); + pp.objects_dist_buffer = getOrDeclareParameter(node, ns_ + ".objects.distance_buffer"); + pp.objects_cut_predicted_paths_beyond_red_lights = + getOrDeclareParameter(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights"); + + pp.overlap_min_dist = getOrDeclareParameter(node, ns_ + ".overlap.minimum_distance"); + pp.overlap_extra_length = getOrDeclareParameter(node, ns_ + ".overlap.extra_length"); + + pp.skip_if_over_max_decel = + getOrDeclareParameter(node, ns_ + ".action.skip_if_over_max_decel"); + pp.precision = getOrDeclareParameter(node, ns_ + ".action.precision"); + pp.min_decision_duration = getOrDeclareParameter(node, ns_ + ".action.min_duration"); + pp.dist_buffer = getOrDeclareParameter(node, ns_ + ".action.distance_buffer"); + pp.slow_velocity = getOrDeclareParameter(node, ns_ + ".action.slowdown.velocity"); + pp.slow_dist_threshold = + getOrDeclareParameter(node, ns_ + ".action.slowdown.distance_threshold"); + pp.stop_dist_threshold = + getOrDeclareParameter(node, ns_ + ".action.stop.distance_threshold"); + + pp.ego_min_velocity = getOrDeclareParameter(node, ns_ + ".ego.min_assumed_velocity"); + pp.extra_front_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_front_offset"); + pp.extra_rear_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_rear_offset"); + pp.extra_left_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_left_offset"); + pp.extra_right_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_right_offset"); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + pp.front_offset = vehicle_info.max_longitudinal_offset_m; + pp.rear_offset = vehicle_info.min_longitudinal_offset_m; + pp.left_offset = vehicle_info.max_lateral_offset_m; + pp.right_offset = vehicle_info.min_lateral_offset_m; +} + +void OutOfLaneModule::update_parameters(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + auto & pp = params_; + updateParam(parameters, ns_ + ".mode", pp.mode); + updateParam(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); + + updateParam(parameters, ns_ + ".threshold.time_threshold", pp.time_threshold); + updateParam(parameters, ns_ + ".intervals.ego_time_buffer", pp.intervals_ego_buffer); + updateParam(parameters, ns_ + ".intervals.objects_time_buffer", pp.intervals_obj_buffer); + updateParam(parameters, ns_ + ".ttc.threshold", pp.ttc_threshold); + + updateParam(parameters, ns_ + ".objects.minimum_velocity", pp.objects_min_vel); + updateParam(parameters, ns_ + ".objects.use_predicted_paths", pp.objects_use_predicted_paths); + updateParam( + parameters, ns_ + ".objects.predicted_path_min_confidence", pp.objects_min_confidence); + updateParam(parameters, ns_ + ".objects.distance_buffer", pp.objects_dist_buffer); + updateParam( + parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights", + pp.objects_cut_predicted_paths_beyond_red_lights); + + updateParam(parameters, ns_ + ".overlap.minimum_distance", pp.overlap_min_dist); + updateParam(parameters, ns_ + ".overlap.extra_length", pp.overlap_extra_length); + + updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel); + updateParam(parameters, ns_ + ".action.precision", pp.precision); + updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); + updateParam(parameters, ns_ + ".action.distance_buffer", pp.dist_buffer); + updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); + updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold); + updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); + + updateParam(parameters, ns_ + ".ego.min_assumed_velocity", pp.ego_min_velocity); + updateParam(parameters, ns_ + ".ego.extra_front_offset", pp.extra_front_offset); + updateParam(parameters, ns_ + ".ego.extra_rear_offset", pp.extra_rear_offset); + updateParam(parameters, ns_ + ".ego.extra_left_offset", pp.extra_left_offset); + updateParam(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset); +} + +VelocityPlanningResult OutOfLaneModule::plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) +{ + VelocityPlanningResult result; + tier4_autoware_utils::StopWatch stopwatch; + stopwatch.tic(); + out_of_lane::EgoData ego_data; + ego_data.pose = planner_data->current_odometry->pose; + ego_data.trajectory_points = ego_trajectory_points; + ego_data.first_trajectory_idx = + motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + ego_data.velocity = planner_data->current_velocity->twist.linear.x; + ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); + stopwatch.tic("calculate_trajectory_footprints"); + const auto current_ego_footprint = + out_of_lane::calculate_current_ego_footprint(ego_data, params_, true); + const auto trajectory_footprints = + out_of_lane::calculate_trajectory_footprints(ego_data, params_); + const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); + // Calculate lanelets to ignore and consider + stopwatch.tic("calculate_lanelets"); + const auto trajectory_lanelets = + out_of_lane::calculate_trajectory_lanelets(ego_data, planner_data->route_handler); + const auto ignored_lanelets = out_of_lane::calculate_ignored_lanelets( + ego_data, trajectory_lanelets, planner_data->route_handler, params_); + const auto other_lanelets = out_of_lane::calculate_other_lanelets( + ego_data, trajectory_lanelets, ignored_lanelets, planner_data->route_handler, params_); + const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); + + debug_data_.reset_data(); + debug_data_.trajectory_points = ego_trajectory_points; + debug_data_.footprints = trajectory_footprints; + debug_data_.trajectory_lanelets = trajectory_lanelets; + debug_data_.ignored_lanelets = ignored_lanelets; + debug_data_.other_lanelets = other_lanelets; + debug_data_.first_trajectory_idx = ego_data.first_trajectory_idx; + + if (params_.skip_if_already_overlapping) { + debug_data_.current_footprint = current_ego_footprint; + const auto overlapped_lanelet_it = + std::find_if(other_lanelets.begin(), other_lanelets.end(), [&](const auto & ll) { + return boost::geometry::intersects(ll.polygon2d().basicPolygon(), current_ego_footprint); + }); + if (overlapped_lanelet_it != other_lanelets.end()) { + debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it); + RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module\n"); + return result; + } + } + // Calculate overlapping ranges + stopwatch.tic("calculate_overlapping_ranges"); + const auto ranges = out_of_lane::calculate_overlapping_ranges( + trajectory_footprints, trajectory_lanelets, other_lanelets, params_); + const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); + // Calculate stop and slowdown points + out_of_lane::DecisionInputs inputs; + inputs.ranges = ranges; + inputs.ego_data = ego_data; + stopwatch.tic("filter_predicted_objects"); + inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_); + const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects"); + inputs.route_handler = planner_data->route_handler; + inputs.lanelets = other_lanelets; + stopwatch.tic("calculate_decisions"); + const auto decisions = out_of_lane::calculate_decisions(inputs, params_, logger_); + const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); + stopwatch.tic("calc_slowdown_points"); + if ( // reset the previous inserted point if the timer expired + prev_inserted_point_ && + (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration) + prev_inserted_point_.reset(); + auto point_to_insert = + out_of_lane::calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_); + const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); + stopwatch.tic("insert_slowdown_points"); + debug_data_.slowdowns.clear(); + if ( // reset the timer if there is no previous inserted point or if we avoid the same lane + point_to_insert && + (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == + point_to_insert->slowdown.lane_to_avoid.id())) + prev_inserted_point_time_ = clock_->now(); + // reuse previous stop point if there is no new one or if its velocity is not higher than the new + // one and its arc length is lower + const auto should_use_prev_inserted_point = [&]() { + if ( + point_to_insert && prev_inserted_point_ && + prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { + const auto arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, point_to_insert->point.pose.position); + const auto prev_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + return prev_arc_length < arc_length; + } + return !point_to_insert && prev_inserted_point_; + }(); + if (should_use_prev_inserted_point) { + // if the trajectory changed the prev point is no longer on the trajectory so we project it + const auto insert_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + prev_inserted_point_->point.pose = + motion_utils::calcInterpolatedPose(ego_trajectory_points, insert_arc_length); + point_to_insert = prev_inserted_point_; + } + if (point_to_insert) { + prev_inserted_point_ = point_to_insert; + RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); + debug_data_.slowdowns = {*point_to_insert}; + if (point_to_insert->slowdown.velocity == 0.0) + result.stop_points.push_back(point_to_insert->point.pose.position); + else + result.slowdown_intervals.emplace_back( + point_to_insert->point.pose.position, point_to_insert->point.pose.position, + point_to_insert->slowdown.velocity); + + const auto is_approaching = motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.pose.position, + point_to_insert->point.pose.position) > 0.1 && + ego_data.velocity > 0.1; + const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING + : motion_utils::VelocityFactor::STOPPED; + velocity_factor_interface_.set( + ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane"); + result.velocity_factor = velocity_factor_interface_.get(); + } else if (!decisions.empty()) { + RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); + } + const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); + debug_data_.ranges = inputs.ranges; + + const auto total_time_us = stopwatch.toc(); + RCLCPP_DEBUG( + logger_, + "Total time = %2.2fus\n" + "\tcalculate_lanelets = %2.0fus\n" + "\tcalculate_trajectory_footprints = %2.0fus\n" + "\tcalculate_overlapping_ranges = %2.0fus\n" + "\tfilter_pred_objects = %2.0fus\n" + "\tcalculate_decisions = %2.0fus\n" + "\tcalc_slowdown_points = %2.0fus\n" + "\tinsert_slowdown_points = %2.0fus\n", + total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us, + calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, + calc_slowdown_points_us, insert_slowdown_points_us); + debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_)); + virtual_wall_marker_creator.add_virtual_walls( + out_of_lane::debug::create_virtual_walls(debug_data_, params_)); + virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); + return result; +} + +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::OutOfLaneModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp new file mode 100644 index 0000000000000..94f1c41fe97a6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OUT_OF_LANE_MODULE_HPP_ +#define OUT_OF_LANE_MODULE_HPP_ + +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class OutOfLaneModule : public PluginModuleInterface +{ +public: + void init(rclcpp::Node & node, const std::string & module_name) override; + void update_parameters(const std::vector & parameters) override; + VelocityPlanningResult plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) override; + std::string get_module_name() const override { return module_name_; } + +private: + void init_parameters(rclcpp::Node & node); + out_of_lane::PlannerParam params_; + + inline static const std::string ns_ = "out_of_lane"; + std::string module_name_; + std::optional prev_inserted_point_{}; + rclcpp::Clock::SharedPtr clock_{}; + rclcpp::Time prev_inserted_point_time_{}; + +protected: + // Debug + mutable out_of_lane::DebugData debug_data_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // OUT_OF_LANE_MODULE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp new file mode 100644 index 0000000000000..f89c620b75fb6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -0,0 +1,127 @@ +// 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 "overlapping_range.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +Overlap calculate_overlap( + const lanelet::BasicPolygon2d & trajectory_footprint, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet) +{ + Overlap overlap; + const auto & left_bound = lanelet.leftBound2d().basicLineString(); + const auto & right_bound = lanelet.rightBound2d().basicLineString(); + // TODO(Maxime): these intersects (for each trajectory footprint, for each lanelet) are too + // expensive + const auto overlap_left = boost::geometry::intersects(trajectory_footprint, left_bound); + const auto overlap_right = boost::geometry::intersects(trajectory_footprint, right_bound); + + lanelet::BasicPolygons2d overlapping_polygons; + if (overlap_left || overlap_right) + boost::geometry::intersection( + trajectory_footprint, lanelet.polygon2d().basicPolygon(), overlapping_polygons); + for (const auto & overlapping_polygon : overlapping_polygons) { + for (const auto & point : overlapping_polygon) { + if (overlap_left && overlap_right) + overlap.inside_distance = boost::geometry::distance(left_bound, right_bound); + else if (overlap_left) + overlap.inside_distance = + std::max(overlap.inside_distance, boost::geometry::distance(point, left_bound)); + else if (overlap_right) + overlap.inside_distance = + std::max(overlap.inside_distance, boost::geometry::distance(point, right_bound)); + geometry_msgs::msg::Pose p; + p.position.x = point.x(); + p.position.y = point.y(); + const auto length = lanelet::utils::getArcCoordinates(trajectory_lanelets, p).length; + if (length > overlap.max_arc_length) { + overlap.max_arc_length = length; + overlap.max_overlap_point = point; + } + if (length < overlap.min_arc_length) { + overlap.min_arc_length = length; + overlap.min_overlap_point = point; + } + } + } + return overlap; +} + +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, + const PlannerParam & params) +{ + OverlapRanges ranges; + OtherLane other_lane(lanelet); + std::vector overlaps; + for (auto i = 0UL; i < trajectory_footprints.size(); ++i) { + const auto overlap = calculate_overlap(trajectory_footprints[i], trajectory_lanelets, lanelet); + const auto has_overlap = overlap.inside_distance > params.overlap_min_dist; + if (has_overlap) { // open/update the range + overlaps.push_back(overlap); + if (!other_lane.range_is_open) { + other_lane.first_range_bound.index = i; + other_lane.first_range_bound.point = overlap.min_overlap_point; + other_lane.first_range_bound.arc_length = + overlap.min_arc_length - params.overlap_extra_length; + other_lane.first_range_bound.inside_distance = overlap.inside_distance; + other_lane.range_is_open = true; + } + other_lane.last_range_bound.index = i; + other_lane.last_range_bound.point = overlap.max_overlap_point; + other_lane.last_range_bound.arc_length = overlap.max_arc_length + params.overlap_extra_length; + other_lane.last_range_bound.inside_distance = overlap.inside_distance; + } else if (other_lane.range_is_open) { // !has_overlap: close the range if it is open + ranges.push_back(other_lane.close_range()); + ranges.back().debug.overlaps = overlaps; + overlaps.clear(); + } + } + // close the range if it is still open + if (other_lane.range_is_open) { + ranges.push_back(other_lane.close_range()); + ranges.back().debug.overlaps = overlaps; + overlaps.clear(); + } + return ranges; +} + +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, + const PlannerParam & params) +{ + OverlapRanges ranges; + for (auto & lanelet : lanelets) { + const auto lanelet_ranges = + calculate_overlapping_ranges(trajectory_footprints, trajectory_lanelets, lanelet, params); + ranges.insert(ranges.end(), lanelet_ranges.begin(), lanelet_ranges.end()); + } + return ranges; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp new file mode 100644 index 0000000000000..07c8663ea21bc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OVERLAPPING_RANGE_HPP_ +#define OVERLAPPING_RANGE_HPP_ + +#include "types.hpp" + +#include + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +/// @brief calculate the overlap between the given footprint and lanelet +/// @param [in] path_footprint footprint used to calculate the overlap +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @param [in] lanelet lanelet used to calculate the overlap +/// @return the found overlap between the footprint and the lanelet +Overlap calculate_overlap( + const lanelet::BasicPolygon2d & trajectory_footprint, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet); +/// @brief calculate the overlapping ranges between the trajectory footprints and a lanelet +/// @param [in] trajectory_footprints footprints used to calculate the overlaps +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @param [in] lanelet lanelet used to calculate the overlaps +/// @param [in] params parameters +/// @return the overlapping ranges found between the footprints and the lanelet +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, + const PlannerParam & params); +/// @brief calculate the overlapping ranges between the trajectory footprints and some lanelets +/// @param [in] trajectory_footprints footprints used to calculate the overlaps +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @param [in] lanelets lanelets used to calculate the overlaps +/// @param [in] params parameters +/// @return the overlapping ranges found between the footprints and the lanelets, sorted by +/// increasing arc length along the trajectory +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp new file mode 100644 index 0000000000000..4fce13582aa1e --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -0,0 +1,236 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPES_HPP_ +#define TYPES_HPP_ + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +using autoware_planning_msgs::msg::TrajectoryPoint; + +/// @brief parameters for the "out of lane" module +struct PlannerParam +{ + std::string mode; // mode used to consider a conflict with an object + bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps + // another lane + + double time_threshold; // [s](mode="threshold") objects time threshold + double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range + double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range + double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object + double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range + + bool objects_use_predicted_paths; // whether to use the objects' predicted paths + bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red + // lights' stop lines + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path + double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in + // the other lane + + double overlap_extra_length; // [m] extra length to add around an overlap range + double overlap_min_dist; // [m] min distance inside another lane to consider an overlap + // action to insert in the trajectory if an object causes a conflict at an overlap + bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel + double dist_buffer; + double slow_velocity; + double slow_dist_threshold; + double stop_dist_threshold; + double precision; // [m] precision when inserting a stop pose in the trajectory + double min_decision_duration; // [s] minimum duration needed a decision can be canceled + // ego dimensions used to create its polygon footprint + double front_offset; // [m] front offset (from vehicle info) + double rear_offset; // [m] rear offset (from vehicle info) + double right_offset; // [m] right offset (from vehicle info) + double left_offset; // [m] left offset (from vehicle info) + double extra_front_offset; // [m] extra front distance + double extra_rear_offset; // [m] extra rear distance + double extra_right_offset; // [m] extra right distance + double extra_left_offset; // [m] extra left distance +}; + +struct EnterExitTimes +{ + double enter_time{}; + double exit_time{}; +}; +struct RangeTimes +{ + EnterExitTimes ego{}; + EnterExitTimes object{}; +}; + +/// @brief action taken by the "out of lane" module +struct Slowdown +{ + size_t target_trajectory_idx{}; // we want to slowdown before this trajectory index + double velocity{}; // desired slow down velocity + lanelet::ConstLanelet lane_to_avoid{}; // we want to slowdown before entering this lane +}; +/// @brief slowdown to insert in a trajectory +struct SlowdownToInsert +{ + Slowdown slowdown{}; + autoware_planning_msgs::msg::TrajectoryPoint point{}; +}; + +/// @brief bound of an overlap range (either the first, or last bound) +struct RangeBound +{ + size_t index{}; + lanelet::BasicPoint2d point{}; + double arc_length{}; + double inside_distance{}; +}; + +/// @brief representation of an overlap between the ego footprint and some other lane +struct Overlap +{ + double inside_distance = 0.0; ///!< distance inside the overlap + double min_arc_length = std::numeric_limits::infinity(); + double max_arc_length = 0.0; + lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length + lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length +}; + +/// @brief range along the trajectory where ego overlaps another lane +struct OverlapRange +{ + lanelet::ConstLanelet lane{}; + size_t entering_trajectory_idx{}; + size_t exiting_trajectory_idx{}; + lanelet::BasicPoint2d entering_point{}; // pose of the overlapping point closest along the lane + lanelet::BasicPoint2d exiting_point{}; // pose of the overlapping point furthest along the lane + double inside_distance{}; // [m] how much ego footprint enters the lane + mutable struct + { + std::vector overlaps{}; + std::optional decision{}; + RangeTimes times{}; + std::optional object{}; + } debug; +}; +using OverlapRanges = std::vector; +/// @brief representation of a lane and its current overlap range +struct OtherLane +{ + bool range_is_open = false; + RangeBound first_range_bound{}; + RangeBound last_range_bound{}; + lanelet::ConstLanelet lanelet{}; + lanelet::BasicPolygon2d polygon{}; + + explicit OtherLane(lanelet::ConstLanelet ll) : lanelet(std::move(ll)) + { + polygon = lanelet.polygon2d().basicPolygon(); + } + + [[nodiscard]] OverlapRange close_range() + { + OverlapRange range; + range.lane = lanelet; + range.entering_trajectory_idx = first_range_bound.index; + range.entering_point = first_range_bound.point; + range.exiting_trajectory_idx = last_range_bound.index; + range.exiting_point = last_range_bound.point; + range.inside_distance = + std::max(first_range_bound.inside_distance, last_range_bound.inside_distance); + range_is_open = false; + last_range_bound = {}; + return range; + } +}; + +/// @brief data related to the ego vehicle +struct EgoData +{ + std::vector trajectory_points{}; + size_t first_trajectory_idx{}; + double velocity{}; // [m/s] + double max_decel{}; // [m/s²] + geometry_msgs::msg::Pose pose{}; +}; + +/// @brief data needed to make decisions +struct DecisionInputs +{ + OverlapRanges ranges{}; + EgoData ego_data{}; + autoware_perception_msgs::msg::PredictedObjects objects{}; + std::shared_ptr route_handler{}; + lanelet::ConstLanelets lanelets{}; +}; + +/// @brief debug data +struct DebugData +{ + std::vector footprints; + std::vector slowdowns; + geometry_msgs::msg::Pose ego_pose; + OverlapRanges ranges; + lanelet::BasicPolygon2d current_footprint; + lanelet::ConstLanelets current_overlapped_lanelets; + lanelet::ConstLanelets trajectory_lanelets; + lanelet::ConstLanelets ignored_lanelets; + lanelet::ConstLanelets other_lanelets; + std::vector trajectory_points; + size_t first_trajectory_idx; + + size_t prev_footprints = 0; + size_t prev_slowdowns = 0; + size_t prev_ranges = 0; + size_t prev_current_overlapped_lanelets = 0; + size_t prev_ignored_lanelets = 0; + size_t prev_trajectory_lanelets = 0; + size_t prev_other_lanelets = 0; + void reset_data() + { + prev_footprints = footprints.size(); + footprints.clear(); + prev_slowdowns = slowdowns.size(); + slowdowns.clear(); + prev_ranges = ranges.size(); + ranges.clear(); + prev_current_overlapped_lanelets = current_overlapped_lanelets.size(); + current_overlapped_lanelets.clear(); + prev_ignored_lanelets = ignored_lanelets.size(); + ignored_lanelets.clear(); + prev_trajectory_lanelets = trajectory_lanelets.size(); + trajectory_lanelets.clear(); + prev_other_lanelets = other_lanelets.size(); + other_lanelets.clear(); + } +}; + +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp new file mode 100644 index 0000000000000..6d1ba8084b821 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp @@ -0,0 +1,58 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/filter_predicted_objects.hpp" + +#include +#include +#include + +#include +#include + +TEST(TestCollisionDistance, CutPredictedPathBeyondLine) +{ + using autoware::motion_velocity_planner::out_of_lane::cut_predicted_path_beyond_line; + autoware_perception_msgs::msg::PredictedPath predicted_path; + lanelet::BasicLineString2d stop_line; + double object_front_overhang = 0.0; + const auto eps = 1e-9; + + EXPECT_NO_THROW(cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang)); + + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + predicted_path.path.push_back(pose); + pose.position.y = 1.0; + predicted_path.path.push_back(pose); + pose.position.y = 2.0; + predicted_path.path.push_back(pose); + pose.position.y = 3.0; + predicted_path.path.push_back(pose); + + cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang); + EXPECT_EQ(predicted_path.path.size(), 4UL); + for (auto i = 0UL; i < predicted_path.path.size(); ++i) { + EXPECT_EQ(predicted_path.path[i].position.x, 0.0); + EXPECT_NEAR(predicted_path.path[i].position.y, static_cast(i), eps); + } + stop_line = {{-1.0, 2.0}, {1.0, 2.0}}; + cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang); + EXPECT_EQ(predicted_path.path.size(), 2UL); + for (auto i = 0UL; i < predicted_path.path.size(); ++i) { + EXPECT_EQ(predicted_path.path[i].position.x, 0.0); + EXPECT_NEAR(predicted_path.path[i].position.y, static_cast(i), eps); + } +} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt new file mode 100644 index 0000000000000..f1eb7ff047fdc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_planner_common) + +find_package(autoware_cmake REQUIRED) + +autoware_package() + +# ament_auto_add_library(${PROJECT_NAME}_lib SHARED +# DIRECTORY src +# ) + +ament_auto_package(INSTALL_TO_SHARE + include +) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp new file mode 100644 index 0000000000000..add6b5ef392ea --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -0,0 +1,106 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; + autoware_perception_msgs::msg::TrafficLightGroup signal; +}; +struct PlannerData +{ + explicit PlannerData(rclcpp::Node & node) + : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) + { + } + + // msgs from callbacks that are used for data-ready + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + pcl::PointCloud::ConstPtr no_ground_pointcloud; + nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; + std::shared_ptr route_handler; + + // nearest search + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + + // other internal data + // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the + // last observed infomation for UNKNOWN + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; + std::optional external_velocity_limit; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; + + // velocity smoother + std::shared_ptr velocity_smoother_{}; + // parameters + vehicle_info_util::VehicleInfo vehicle_info_; + + /** + *@fn + *@brief queries the traffic signal information of given Id. if keep_last_observation = true, + *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation + */ + std::optional get_traffic_signal( + const lanelet::Id id, const bool keep_last_observation = false) const + { + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; + if (traffic_light_id_map.count(id) == 0) { + return std::nullopt; + } + return std::make_optional(traffic_light_id_map.at(id)); + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp new file mode 100644 index 0000000000000..dd2b89e3261d6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 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. + +#ifndef AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ + +#include "planner_data.hpp" +#include "velocity_planning_result.hpp" + +#include +#include + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ + +class PluginModuleInterface +{ +public: + virtual ~PluginModuleInterface() = default; + virtual void init(rclcpp::Node & node, const std::string & module_name) = 0; + virtual void update_parameters(const std::vector & parameters) = 0; + virtual VelocityPlanningResult plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) = 0; + virtual std::string get_module_name() const = 0; + motion_utils::VelocityFactorInterface velocity_factor_interface_; + rclcpp::Logger logger_ = rclcpp::get_logger(""); + rclcpp::Publisher::SharedPtr debug_publisher_; + rclcpp::Publisher::SharedPtr virtual_wall_publisher_; + motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp new file mode 100644 index 0000000000000..9b011b74503f2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct SlowdownInterval +{ + SlowdownInterval( + const geometry_msgs::msg::Point & from_, const geometry_msgs::msg::Point & to_, + const double vel) + : from{from_}, to{to_}, velocity{vel} + { + } + geometry_msgs::msg::Point from{}; + geometry_msgs::msg::Point to{}; + double velocity{}; +}; +struct VelocityPlanningResult +{ + std::vector stop_points{}; + std::vector slowdown_intervals{}; + std::optional velocity_factor{}; +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ 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 new file mode 100644 index 0000000000000..2f4e7241d60ee --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -0,0 +1,40 @@ + + + + autoware_motion_velocity_planner_common + 0.1.0 + Common functions and interfaces for motion_velocity_planner modules + + Maxime Clement + + Apache License 2.0 + + Maxime Clement + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_perception_msgs + autoware_planning_msgs + autoware_velocity_smoother + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + rclcpp + route_handler + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt new file mode 100644 index 0000000000000..0af4da6fd4426 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_planner_node) + +find_package(autoware_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "srv/LoadPlugin.srv" + "srv/UnloadPlugin.srv" + DEPENDENCIES +) + +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + DIRECTORY src +) + +rclcpp_components_register_node(${PROJECT_NAME}_lib + PLUGIN "autoware::motion_velocity_planner::MotionVelocityPlannerNode" + EXECUTABLE ${PROJECT_NAME}_exe +) + +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(${PROJECT_NAME}_lib + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") +endif() + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME}_lib + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md new file mode 100644 index 0000000000000..494446828e134 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -0,0 +1,58 @@ +# Motion Velocity Planner + +## Overview + +`motion_velocity_planner` is a planner to adjust the trajectory velocity based on the obstacles around the vehicle. +It loads modules as plugins. Please refer to the links listed below for detail on each module. + +![Architecture](./docs/MotionVelocityPlanner-InternalInterface.drawio.svg) + +- [Out of Lane](../autoware_motion_velocity_out_of_lane_module/README.md) + +Each module calculates stop and slow down points to be inserted in the ego trajectory. +These points are assumed to correspond to the `base_link` frame of the ego vehicle as it follows the trajectory. +This means that to stop before a wall, a stop point is inserted in the trajectory at a distance ahead of the wall equal to the vehicle front offset (wheelbase + front overhang, see the [vehicle dimensions](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/). + +![set_stop_velocity](./docs/set_stop_velocity.drawio.svg) + +## Input topics + +| Name | Type | Description | +| -------------------------------------- | ----------------------------------------------------- | ----------------------------- | +| `~/input/trajectory` | autoware_planning_msgs::msg::Trajectory | input trajectory | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | +| `~/input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states | +| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray | virtual traffic light states | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | + +## Output topics + +| Name | Type | Description | +| --------------------------- | ------------------------------------------------- | -------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile | + +## Services + +| Name | Type | Description | +| ------------------------- | -------------------------------------------------------- | ---------------------------- | +| `~/service/load_plugin` | autoware_motion_velocity_planner_node::srv::LoadPlugin | To request loading a plugin | +| `~/service/unload_plugin` | autoware_motion_velocity_planner_node::srv::UnloadPlugin | To request unloaded a plugin | + +## Node parameters + +| Parameter | Type | Description | +| ---------------- | ---------------- | ---------------------- | +| `launch_modules` | vector\ | module names to launch | + +In addition, the following parameters should be provided to the node: + +- [nearest search parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/nearest_search.param.yaml); +- [vehicle info parameters](https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml); +- [common planning parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml); +- [smoother parameters](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_velocity_smoother/#parameters) +- Parameters of each plugin that will be loaded. diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml new file mode 100644 index 0000000000000..5b2fea537d4f7 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + smooth_velocity_before_planning: true # [-] if true, smooth the velocity profile of the input trajectory before planning diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg new file mode 100644 index 0000000000000..ea3b7ad99d4e5 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg @@ -0,0 +1,538 @@ + + + + + + + + + + + + + + + +
+
+
+ ~/input/dynamic_objects +
+
+
+ + + + + + + + + + + + + +
+
+
+ ~/input/vehicle_odometry +
+
+
+ + + + + + + + + + + + + +
+
+
+ ~/input/trajectory +
+
+
+ + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + + + + + Motion Velocity Planner + + + + + + + + + +
+
+
+ PlanVelocity +
+
+
+ + + + + + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + + + + + +
+
+
+ + Updated +
+ Trajectory + +
+
+
+ + + + + + + + + + + + + +
+
+
+
Stop/Slowdown
+
+ points +
+
+
+
+
+ + + + + + + + + + + + + +
+
+
+ Data +
+
+
+ + + + + + + + + + + + +
+
+
+ Planner Data +
+
+
+ + + + + + + + + + + + + Loaded Modules + + + + + + + + + + out_of_lane + + + + + + + + + + obstacle_velocity_limiter + + + + + + + + + + dynamic_obstacle_stop + + + + + + + + + +
+
+
...
+
+
+ + + + + + + + + + + + +
+
+
+ Smooth Velocity +
+
+
+ + + + + + + + + + + + + obstacle_stop + + + + + + + + + + +
+
+
+ ~/input/vector_map +
+
+
+ + + + + + + + + + + + +
+
+
+ + ~/output/trajectory + +
+
+
+ + + + + + + + + + + + + +
+
+
+ ... +
+
+
+ + + + + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg new file mode 100644 index 0000000000000..5ae7d7dbb5461 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg @@ -0,0 +1,197 @@ + + + + + + + + + + + +
+
+
+ +

+ + stop point + +

+ +
+
+
+ + stop point + + + + + + + + + + + +
+
+
+ +

+ + v + +

+ +
+
+
+ + v + + + + + + +
+
+
+ +

+ + x + +

+ +
+
+
+ + x + + + + + + +
+
+
+ +

+ + baselink to front + +

+ +
+
+
+ + baselink to front + + + + + + + + + + + + + + +
+
+
+ +

+ + output velocity + +

+ +
+
+
+ + output velocity + + + + + + + +
+
+
+ +

+ + reference velocity + +

+ +
+
+
+ + reference velocity + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml new file mode 100644 index 0000000000000..3732d86ef380a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml new file mode 100644 index 0000000000000..e45935b36b13a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -0,0 +1,57 @@ + + + + autoware_motion_velocity_planner_node + 0.1.0 + Node of the motion_velocity_planner + + Maxime Clement + + Apache License 2.0 + + Maxime Clement + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + rosidl_default_generators + + autoware_map_msgs + autoware_motion_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs + autoware_velocity_smoother + diagnostic_msgs + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pcl_conversions + pluginlib + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + rosidl_default_runtime + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_motion_velocity_out_of_lane_module + autoware_planning_test_manager + + rosidl_interface_packages + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json new file mode 100644 index 0000000000000..7db22e5e39d17 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for the Motion Velocity Planner", + "type": "object", + "definitions": { + "motion_velocity_planner": { + "type": "object", + "properties": { + "smooth_velocity_before_planning": { + "type": "boolean", + "default": true, + "description": "if true, smooth the velocity profile of the input trajectory before planning" + } + }, + "required": ["smooth_velocity_before_planning"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/motion_velocity_planner" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} 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 new file mode 100644 index 0000000000000..4d152afa250b7 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -0,0 +1,460 @@ +// 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 "node.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace +{ +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node_ptr) +{ + rclcpp::CallbackGroup::SharedPtr callback_group = + node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group; + + return sub_opt; +} +} // namespace + +namespace autoware::motion_velocity_planner +{ +MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & node_options) +: Node("motion_velocity_planner_node", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + planner_data_(*this) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + // Subscribers + sub_trajectory_ = this->create_subscription( + "~/input/trajectory", 1, std::bind(&MotionVelocityPlannerNode::on_trajectory, this, _1), + create_subscription_options(this)); + sub_predicted_objects_ = + this->create_subscription( + "~/input/dynamic_objects", 1, + std::bind(&MotionVelocityPlannerNode::on_predicted_objects, this, _1), + create_subscription_options(this)); + sub_no_ground_pointcloud_ = this->create_subscription( + "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), + std::bind(&MotionVelocityPlannerNode::on_no_ground_pointcloud, this, _1), + create_subscription_options(this)); + sub_vehicle_odometry_ = this->create_subscription( + "~/input/vehicle_odometry", 1, std::bind(&MotionVelocityPlannerNode::on_odometry, this, _1), + create_subscription_options(this)); + sub_acceleration_ = this->create_subscription( + "~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1), + create_subscription_options(this)); + sub_lanelet_map_ = this->create_subscription( + "~/input/vector_map", rclcpp::QoS(10).transient_local(), + std::bind(&MotionVelocityPlannerNode::on_lanelet_map, this, _1), + create_subscription_options(this)); + sub_traffic_signals_ = + this->create_subscription( + "~/input/traffic_signals", 1, + std::bind(&MotionVelocityPlannerNode::on_traffic_signals, this, _1), + create_subscription_options(this)); + sub_virtual_traffic_light_states_ = + this->create_subscription( + "~/input/virtual_traffic_light_states", 1, + std::bind(&MotionVelocityPlannerNode::on_virtual_traffic_light_states, this, _1), + create_subscription_options(this)); + sub_occupancy_grid_ = this->create_subscription( + "~/input/occupancy_grid", 1, std::bind(&MotionVelocityPlannerNode::on_occupancy_grid, this, _1), + create_subscription_options(this)); + + srv_load_plugin_ = create_service( + "~/service/load_plugin", std::bind(&MotionVelocityPlannerNode::on_load_plugin, this, _1, _2)); + srv_unload_plugin_ = create_service( + "~/service/unload_plugin", + std::bind(&MotionVelocityPlannerNode::on_unload_plugin, this, _1, _2)); + + // Publishers + trajectory_pub_ = + this->create_publisher("~/output/trajectory", 1); + velocity_factor_publisher_ = + this->create_publisher( + "~/output/velocity_factors", 1); + + // Parameters + smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); + // nearest search + planner_data_.ego_nearest_dist_threshold = + declare_parameter("ego_nearest_dist_threshold"); + planner_data_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + // set velocity smoother param + set_velocity_smoother_params(); + + // Initialize PlannerManager + for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } + planner_manager_.load_module_plugin(*this, name); + } + + set_param_callback_ = this->add_on_set_parameters_callback( + std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); +} + +void MotionVelocityPlannerNode::on_load_plugin( + const LoadPlugin::Request::SharedPtr request, + [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.load_module_plugin(*this, request->plugin_name); +} + +void MotionVelocityPlannerNode::on_unload_plugin( + const UnloadPlugin::Request::SharedPtr request, + [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.unload_module_plugin(*this, request->plugin_name); +} + +// NOTE: argument planner_data must not be referenced for multithreading +bool MotionVelocityPlannerNode::is_data_ready() const +{ + const auto & d = planner_data_; + auto clock = *get_clock(); + const auto check_with_msg = [&](const auto ptr, const auto & msg) { + constexpr auto throttle_duration = 3000; // [ms] + if (!ptr) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, msg); + return false; + } + return true; + }; + + return check_with_msg(d.current_odometry, "Waiting for current odometry") && + check_with_msg(d.current_velocity, "Waiting for current velocity") && + check_with_msg(d.current_acceleration, "Waiting for current acceleration") && + check_with_msg(d.predicted_objects, "Waiting for predicted objects") && + check_with_msg(d.no_ground_pointcloud, "Waiting for pointcloud") && + check_with_msg(map_ptr_, "Waiting for the map") && + check_with_msg( + d.velocity_smoother_, "Waiting for the initialization of the velocity smoother") && + check_with_msg(d.occupancy_grid, "Waiting for the occupancy grid"); +} + +void MotionVelocityPlannerNode::on_occupancy_grid( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.occupancy_grid = msg; +} + +void MotionVelocityPlannerNode::on_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.predicted_objects = msg; +} + +void MotionVelocityPlannerNode::on_no_ground_pointcloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_.lookupTransform( + "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & e) { + RCLCPP_WARN(get_logger(), "no transform found for no_ground_pointcloud: %s", e.what()); + return; + } + + pcl::PointCloud pc; + pcl::fromROSMsg(*msg, pc); + + Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); + pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); + if (!pc.empty()) { + tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); + } + + { + std::lock_guard lock(mutex_); + planner_data_.no_ground_pointcloud = pc_transformed; + } +} + +void MotionVelocityPlannerNode::on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + auto current_odometry = std::make_shared(); + current_odometry->header = msg->header; + current_odometry->pose = msg->pose.pose; + planner_data_.current_odometry = current_odometry; + + auto current_velocity = std::make_shared(); + current_velocity->header = msg->header; + current_velocity->twist = msg->twist.twist; + planner_data_.current_velocity = current_velocity; +} + +void MotionVelocityPlannerNode::on_acceleration( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.current_acceleration = msg; +} + +void MotionVelocityPlannerNode::set_velocity_smoother_params() +{ + planner_data_.velocity_smoother_ = + std::make_shared(*this); +} + +void MotionVelocityPlannerNode::on_lanelet_map( + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + map_ptr_ = msg; + has_received_map_ = true; +} + +void MotionVelocityPlannerNode::on_traffic_signals( + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + // clear previous observation + planner_data_.traffic_light_id_map_raw_.clear(); + const auto traffic_light_id_map_last_observed_old = + planner_data_.traffic_light_id_map_last_observed_; + planner_data_.traffic_light_id_map_last_observed_.clear(); + for (const auto & signal : msg->traffic_light_groups) { + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; + traffic_signal.signal = signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_light_group_id] = traffic_signal; + const bool is_unknown_observation = + std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { + return element.color == autoware_perception_msgs::msg::TrafficLightElement::UNKNOWN; + }); + // if the observation is UNKNOWN and past observation is available, only update the timestamp + // and keep the body of the info + const auto old_data = + traffic_light_id_map_last_observed_old.find(signal.traffic_light_group_id); + if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { + // copy last observation + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = + old_data->second; + // update timestamp + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id].stamp = + msg->stamp; + } else { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = + traffic_signal; + } + } +} + +void MotionVelocityPlannerNode::on_virtual_traffic_light_states( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.virtual_traffic_light_states = msg; +} + +void MotionVelocityPlannerNode::on_trajectory( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg) +{ + std::unique_lock lk(mutex_); + + if (!is_data_ready()) { + return; + } + + if (input_trajectory_msg->points.empty()) { + RCLCPP_WARN(get_logger(), "Input trajectory message is empty"); + return; + } + + if (has_received_map_) { + planner_data_.route_handler = std::make_shared(*map_ptr_); + has_received_map_ = false; + } + + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{ + input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()}; + + auto output_trajectory_msg = generate_trajectory(input_trajectory_points); + output_trajectory_msg.header = input_trajectory_msg->header; + + lk.unlock(); + + trajectory_pub_->publish(output_trajectory_msg); + published_time_publisher_->publish_if_subscribed( + trajectory_pub_, output_trajectory_msg.header.stamp); +} + +void MotionVelocityPlannerNode::insert_stop( + autoware_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Point & stop_point) const +{ + const auto seg_idx = motion_utils::findNearestSegmentIndex(trajectory.points, stop_point); + const auto insert_idx = motion_utils::insertTargetPoint(seg_idx, stop_point, trajectory.points); + if (insert_idx) { + for (auto idx = *insert_idx; idx < trajectory.points.size(); ++idx) + trajectory.points[idx].longitudinal_velocity_mps = 0.0; + } else { + RCLCPP_WARN(get_logger(), "Failed to insert stop point"); + } +} + +void MotionVelocityPlannerNode::insert_slowdown( + autoware_planning_msgs::msg::Trajectory & trajectory, + const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const +{ + const auto from_seg_idx = + motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.from); + const auto from_insert_idx = + motion_utils::insertTargetPoint(from_seg_idx, slowdown_interval.from, trajectory.points); + const auto to_seg_idx = + motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.to); + const auto to_insert_idx = + motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); + if (from_insert_idx && to_insert_idx) { + for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) + trajectory.points[idx].longitudinal_velocity_mps = 0.0; + } else { + RCLCPP_WARN(get_logger(), "Failed to insert slowdown point"); + } +} + +autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::smooth_trajectory( + const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, + const autoware::motion_velocity_planner::PlannerData & planner_data) const +{ + const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry->pose; + const double v0 = planner_data.current_velocity->twist.linear.x; + const double a0 = planner_data.current_acceleration->accel.accel.linear.x; + const auto & external_v_limit = planner_data.external_velocity_limit; + const auto & smoother = planner_data.velocity_smoother_; + + const auto traj_lateral_acc_filtered = + smoother->applyLateralAccelerationFilter(trajectory_points); + + const auto traj_steering_rate_limited = + smoother->applySteeringRateLimit(traj_lateral_acc_filtered, false); + + // Resample trajectory with ego-velocity based interval distances + auto traj_resampled = smoother->resampleTrajectory( + traj_steering_rate_limited, v0, current_pose, planner_data.ego_nearest_dist_threshold, + planner_data.ego_nearest_yaw_threshold); + const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_resampled, current_pose, planner_data.ego_nearest_dist_threshold, + planner_data.ego_nearest_yaw_threshold); + std::vector debug_trajectories; + // Clip trajectory from closest point + autoware::motion_velocity_planner::TrajectoryPoints clipped; + autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed; + clipped.insert( + clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + RCLCPP_ERROR(get_logger(), "failed to smooth"); + } + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); + + if (external_v_limit) { + autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + } + return traj_smoothed; +} + +autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points) +{ + autoware_planning_msgs::msg::Trajectory output_trajectory_msg; + output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; + if (smooth_velocity_before_planning_) + input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); + + const auto planning_results = planner_manager_.plan_velocities( + input_trajectory_points, std::make_shared(planner_data_)); + + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; + velocity_factors.header.frame_id = "map"; + velocity_factors.header.stamp = get_clock()->now(); + + for (const auto & planning_result : planning_results) { + for (const auto & stop_point : planning_result.stop_points) + insert_stop(output_trajectory_msg, stop_point); + for (const auto & slowdown_interval : planning_result.slowdown_intervals) + insert_slowdown(output_trajectory_msg, slowdown_interval); + if (planning_result.velocity_factor) + velocity_factors.factors.push_back(*planning_result.velocity_factor); + } + + velocity_factor_publisher_->publish(velocity_factors); + return output_trajectory_msg; +} + +rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + { + std::unique_lock lk(mutex_); // for planner_manager_ + planner_manager_.update_module_parameters(parameters); + } + + updateParam(parameters, "smooth_velocity_before_planning", smooth_velocity_before_planning_); + updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold); + updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold); + + set_velocity_smoother_params(); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} +} // namespace autoware::motion_velocity_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion_velocity_planner::MotionVelocityPlannerNode) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp new file mode 100644 index 0000000000000..4b943dcbb67f2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -0,0 +1,140 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NODE_HPP_ +#define NODE_HPP_ + +#include "planner_manager.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_motion_velocity_planner_node::srv::LoadPlugin; +using autoware_motion_velocity_planner_node::srv::UnloadPlugin; +using autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoints = std::vector; + +class MotionVelocityPlannerNode : public rclcpp::Node +{ +public: + explicit MotionVelocityPlannerNode(const rclcpp::NodeOptions & node_options); + +private: + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // subscriber + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr + sub_predicted_objects_; + rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; + rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; + rclcpp::Subscription::SharedPtr sub_acceleration_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr + sub_traffic_signals_; + rclcpp::Subscription::SharedPtr + sub_virtual_traffic_light_states_; + rclcpp::Subscription::SharedPtr sub_occupancy_grid_; + + void on_trajectory( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg); + void on_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + void on_no_ground_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + void on_lanelet_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); + void on_traffic_signals( + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); + void on_virtual_traffic_light_states( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); + void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); + + // publishers + rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr + velocity_factor_publisher_; + + // parameters + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; + bool smooth_velocity_before_planning_{}; + /// @brief set parameters of the velocity smoother + void set_velocity_smoother_params(); + + // members + PlannerData planner_data_; + MotionVelocityPlannerManager planner_manager_; + LaneletMapBin::ConstSharedPtr map_ptr_{nullptr}; + bool has_received_map_ = false; + + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; + void on_unload_plugin( + const UnloadPlugin::Request::SharedPtr request, + const UnloadPlugin::Response::SharedPtr response); + void on_load_plugin( + const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + rcl_interfaces::msg::SetParametersResult on_set_param( + const std::vector & parameters); + + // mutex for planner_data_ + std::mutex mutex_; + + // function + bool is_data_ready() const; + void insert_stop( + autoware_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Point & stop_point) const; + void insert_slowdown( + autoware_planning_msgs::msg::Trajectory & trajectory, + const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const; + autoware::motion_velocity_planner::TrajectoryPoints smooth_trajectory( + const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, + const autoware::motion_velocity_planner::PlannerData & planner_data) const; + autoware_planning_msgs::msg::Trajectory generate_trajectory( + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); + + std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // NODE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp new file mode 100644 index 0000000000000..66063fcbaebca --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -0,0 +1,83 @@ +// 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 "planner_manager.hpp" + +#include + +#include +#include + +namespace autoware::motion_velocity_planner +{ + +MotionVelocityPlannerManager::MotionVelocityPlannerManager() +: plugin_loader_( + "autoware_motion_velocity_planner_node", + "autoware::motion_velocity_planner::PluginModuleInterface") +{ +} + +void MotionVelocityPlannerManager::load_module_plugin(rclcpp::Node & node, const std::string & name) +{ + // Check if the plugin is already loaded. + if (plugin_loader_.isClassLoaded(name)) { + RCLCPP_WARN_STREAM(node.get_logger(), "The plugin '" << name << "' is already loaded."); + return; + } + if (plugin_loader_.isClassAvailable(name)) { + const auto plugin = plugin_loader_.createSharedInstance(name); + plugin->init(node, name); + + // register + loaded_plugins_.push_back(plugin); + RCLCPP_DEBUG_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); + } else { + RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); + } +} + +void MotionVelocityPlannerManager::unload_module_plugin( + rclcpp::Node & node, const std::string & name) +{ + auto it = std::remove_if(loaded_plugins_.begin(), loaded_plugins_.end(), [&](const auto plugin) { + return plugin->get_module_name() == name; + }); + + if (it == loaded_plugins_.end()) { + RCLCPP_WARN_STREAM( + node.get_logger(), + "The scene plugin '" << name << "' is not found in the registered modules."); + } else { + loaded_plugins_.erase(it, loaded_plugins_.end()); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is unloaded."); + } +} + +void MotionVelocityPlannerManager::update_module_parameters( + const std::vector & parameters) +{ + for (auto & plugin : loaded_plugins_) plugin->update_parameters(parameters); +} + +std::vector MotionVelocityPlannerManager::plan_velocities( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) +{ + std::vector results; + for (auto & plugin : loaded_plugins_) + results.push_back(plugin->plan(ego_trajectory_points, planner_data)); + return results; +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp new file mode 100644 index 0000000000000..70af1041ec222 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PLANNER_MANAGER_HPP_ +#define PLANNER_MANAGER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class MotionVelocityPlannerManager +{ +public: + MotionVelocityPlannerManager(); + void load_module_plugin(rclcpp::Node & node, const std::string & name); + void unload_module_plugin(rclcpp::Node & node, const std::string & name); + void update_module_parameters(const std::vector & parameters); + std::vector plan_velocities( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data); + +private: + pluginlib::ClassLoader plugin_loader_; + std::vector> loaded_plugins_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // PLANNER_MANAGER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv new file mode 100644 index 0000000000000..7b9f08ab0ded8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv new file mode 100644 index 0000000000000..7b9f08ab0ded8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp new file mode 100644 index 0000000000000..46887a56c84f2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp @@ -0,0 +1,138 @@ +// 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 "node.hpp" + +#include +#include +#include + +#include + +#include + +using autoware::motion_velocity_planner::MotionVelocityPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: motion_velocity_planner → test_node_ + test_manager->setTrajectorySubscriber("motion_velocity_planner_node/output/trajectory"); + + // set motion_velocity_planner node's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("motion_velocity_planner_node/input/trajectory"); + + test_manager->setInitialPoseTopicName("motion_velocity_planner_node/input/vehicle_odometry"); + test_manager->setOdometryTopicName("motion_velocity_planner_node/input/vehicle_odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto motion_velocity_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_motion_velocity_planner_node"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); + + const auto get_motion_velocity_module_config = [](const std::string & module) { + const auto package_name = "autoware_motion_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + + std::vector module_names; + module_names.emplace_back("autoware::motion_velocity_planner::OutOfLaneModule"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + params.emplace_back("is_simulation", false); + 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", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + motion_velocity_planner_dir + "/config/motion_velocity_planner.param.yaml", + get_motion_velocity_module_config("out_of_lane")}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishTF(test_target_node, "/tf"); + test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); + test_manager->publishPredictedObjects( + test_target_node, "motion_velocity_planner_node/input/dynamic_objects"); + test_manager->publishPointCloud( + test_target_node, "motion_velocity_planner_node/input/no_ground_pointcloud"); + test_manager->publishOdometry( + test_target_node, "motion_velocity_planner_node/input/vehicle_odometry"); + test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); + test_manager->publishMap(test_target_node, "motion_velocity_planner_node/input/vector_map"); + test_manager->publishTrafficSignals( + test_target_node, "motion_velocity_planner_node/input/traffic_signals"); + test_manager->publishVirtualTrafficLightState( + test_target_node, "motion_velocity_planner_node/input/virtual_traffic_light_states"); + test_manager->publishOccupancyGrid( + test_target_node, "motion_velocity_planner_node/input/occupancy_grid"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + + // make sure motion_velocity_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromTrajectory(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp index 32262ab7eec35..6a6bd2c0dcf19 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp @@ -15,7 +15,7 @@ #ifndef OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ #define OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ -#include +#include #include #include @@ -24,7 +24,7 @@ namespace objects_of_interest_marker_interface struct ObjectMarkerData { geometry_msgs::msg::Pose pose{}; - autoware_auto_perception_msgs::msg::Shape shape{}; + autoware_perception_msgs::msg::Shape shape{}; std_msgs::msg::ColorRGBA color; }; diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp index c1fae5ecc4bec..5a094e349c31a 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp index d5a19fede4fc2..7138ddd49d4c7 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include #include @@ -47,7 +47,7 @@ class ObjectsOfInterestMarkerInterface * @param color_name Color name */ void insertObjectData( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name); /** @@ -57,7 +57,7 @@ class ObjectsOfInterestMarkerInterface * @param color Color data with alpha */ void insertObjectDataWithCustomColor( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const std_msgs::msg::ColorRGBA & color); /** diff --git a/planning/objects_of_interest_marker_interface/package.xml b/planning/objects_of_interest_marker_interface/package.xml index 2cdb0fe2c9a46..28d00e2fdcd95 100644 --- a/planning/objects_of_interest_marker_interface/package.xml +++ b/planning/objects_of_interest_marker_interface/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs rclcpp std_msgs diff --git a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp index 03e71193cc5a5..f12731aba5df2 100644 --- a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -20,7 +20,7 @@ namespace objects_of_interest_marker_interface { -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using std_msgs::msg::ColorRGBA; using visualization_msgs::msg::Marker; diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 8016107d96911..23be6908cbcfe 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -15,17 +15,17 @@ The `obstacle_cruise_planner` package has following modules. ### Input topics -| Name | Type | Description | -| -------------------- | ----------------------------------------------- | ---------------- | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | -| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | +| Name | Type | Description | +| -------------------- | ------------------------------------------ | ---------------- | +| `~/input/trajectory` | autoware_planning_msgs::Trajectory | input trajectory | +| `~/input/objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | ### Output topics | Name | Type | Description | | ------------------------------- | ---------------------------------------------- | ------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory | +| `~/output/trajectory` | autoware_planning_msgs::Trajectory | output trajectory | | `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | | `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | | `~/output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | @@ -46,7 +46,7 @@ This planner data is created first, and then sent to the planning algorithm. struct PlannerData { rclcpp::Time current_time; - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; geometry_msgs::msg::Pose current_pose; double ego_vel; double current_acc; 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 bd9c5750af78f..2ccd000657948 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -87,7 +87,8 @@ obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop stop: - max_lat_margin: 0.3 # lateral margin between obstacle and trajectory band with ego's width + 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 crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] 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 3f18c6e1728c6..fd65446408db1 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -200,6 +200,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double prediction_resampling_time_horizon; // max lateral margin double max_lat_margin_for_stop; + double max_lat_margin_for_stop_against_unknown; double max_lat_margin_for_cruise; double max_lat_margin_for_slow_down; double lat_hysteresis_margin_for_slow_down; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp index d46062dd8f85c..979cef8610cbd 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -19,9 +19,9 @@ #include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp" #include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,13 +38,13 @@ using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::AccelStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index eaba45a31869a..e394add65b05d 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -18,8 +18,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py index 6a11c916124e9..e3ede845a842d 100755 --- a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py +++ b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py @@ -15,7 +15,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import Trajectory from geometry_msgs.msg import Pose from geometry_msgs.msg import Twist from geometry_msgs.msg import TwistStamped diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index f637911dae903..c5f0c73a13b78 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -247,6 +247,8 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara max_lat_margin_for_stop = node.declare_parameter("behavior_determination.stop.max_lat_margin"); + max_lat_margin_for_stop_against_unknown = + node.declare_parameter("behavior_determination.stop.max_lat_margin_against_unknown"); max_lat_margin_for_cruise = node.declare_parameter("behavior_determination.cruise.max_lat_margin"); enable_yield = node.declare_parameter("behavior_determination.cruise.yield.enable_yield"); @@ -311,6 +313,9 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( tier4_autoware_utils::updateParam( parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.stop.max_lat_margin_against_unknown", + max_lat_margin_for_stop_against_unknown); tier4_autoware_utils::updateParam( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); tier4_autoware_utils::updateParam( @@ -682,8 +687,8 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( }(); const double max_lat_margin = std::max( - std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_cruise), - p.max_lat_margin_for_slow_down); + {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, + p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); if (max_lat_margin < min_lat_dist_to_traj_poly) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1174,7 +1179,13 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( if (!isStopObstacle(obstacle.classification.label)) { return std::nullopt; } - if (p.max_lat_margin_for_stop < precise_lat_dist) { + + const double max_lat_margin_for_stop = + (obstacle.classification.label == ObjectClassification::UNKNOWN) + ? p.max_lat_margin_for_stop_against_unknown + : p.max_lat_margin_for_stop; + + if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) { return std::nullopt; } @@ -1207,7 +1218,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + std::hypot( vehicle_info_.vehicle_length_m, - vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_stop)); + vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); if (collision_points.empty()) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1232,8 +1243,8 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } // calculate collision points with trajectory with lateral stop margin - const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, odometry.pose.pose, p.max_lat_margin_for_stop); + const auto traj_polys_with_lat_margin = + createOneStepPolygons(traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop); const auto collision_point = polygon_utils::getCollisionPoint( traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); @@ -1378,7 +1389,6 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto current_closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - // If previous closest obstacle ptr is not set if (!prev_closest_stop_obstacle_ptr_) { if (current_closest_stop_obstacle) { prev_closest_stop_obstacle_ptr_ = @@ -1387,44 +1397,23 @@ void ObstacleCruisePlannerNode::checkConsistency( return; } - // Put previous closest target obstacle if necessary 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 is not in the current perception lists - // just return the current target obstacles + // If previous closest obstacle disappear from the perception result, do nothing anymore. if (predicted_object_itr == predicted_objects.objects.end()) { return; } - // Previous closest obstacle is in the perception lists - const auto obstacle_itr = std::find_if( + 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; }); - - // Previous closest obstacle is both in the perception lists and target obstacles - if (obstacle_itr != stop_obstacles.end()) { - if (current_closest_stop_obstacle) { - if ((current_closest_stop_obstacle->uuid == prev_closest_stop_obstacle_ptr_->uuid)) { - // prev_closest_obstacle is current_closest_stop_obstacle just return the target - // obstacles(in target obstacles) - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - // New obstacle becomes new stop obstacle - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } - } else { - // Previous closest stop obstacle becomes cruise obstacle - prev_closest_stop_obstacle_ptr_ = nullptr; - } - } else { - // prev obstacle is not in the target obstacles, but in the perception list + 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 < @@ -1433,13 +1422,13 @@ void ObstacleCruisePlannerNode::checkConsistency( stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_); return; } + } - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - prev_closest_stop_obstacle_ptr_ = nullptr; - } + if (current_closest_stop_obstacle) { + prev_closest_stop_obstacle_ptr_ = + std::make_shared(*current_closest_stop_obstacle); + } else { + prev_closest_stop_obstacle_ptr_ = nullptr; } } diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index ef5b0facd236e..c922874594f54 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -13,21 +13,21 @@ ### Input topics -| Name | Type | Description | -| --------------------------- | ----------------------------------------------- | ------------------- | -| `~/input/pointcloud` | sensor_msgs::PointCloud2 | obstacle pointcloud | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory | -| `~/input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map | -| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | -| `~/input/dynamic_objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | +| Name | Type | Description | +| --------------------------- | ------------------------------------------ | ------------------- | +| `~/input/pointcloud` | sensor_msgs::PointCloud2 | obstacle pointcloud | +| `~/input/trajectory` | autoware_planning_msgs::Trajectory | trajectory | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | +| `~/input/dynamic_objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | ### Output topics -| Name | Type | Description | -| ---------------------- | --------------------------------------- | -------------------------------------- | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | -| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop | +| Name | Type | Description | +| ---------------------- | ------------------------------------ | -------------------------------------- | +| `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | +| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop | ### Common Parameter diff --git a/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml b/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml index 92b56c2d2e6e4..4c47fa994cc9b 100644 --- a/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml +++ b/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml @@ -8,10 +8,10 @@ - - - - + + + + @@ -69,10 +69,10 @@ - - - - + + + + diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 2c7308e576129..55501cf596b07 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -17,8 +17,8 @@ #include -#include -#include +#include +#include #include #include @@ -31,9 +31,9 @@ namespace motion_planning { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; class AdaptiveCruiseController { public: @@ -45,7 +45,7 @@ class AdaptiveCruiseController const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header); @@ -192,7 +192,7 @@ class AdaptiveCruiseController const std_msgs::msg::Header & trajectory_header); double calcTrajYaw(const TrajectoryPoints & trajectory, const int collision_point_idx); std::optional estimatePointVelocityFromObject( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const double traj_yaw, const pcl::PointXYZ & nearest_collision_point); std::optional estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index d894544a67fe1..723d7e8d12e56 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -29,8 +29,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -77,9 +77,9 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; @@ -94,7 +94,7 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; struct ObstacleWithDetectionTime { explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 6b8e2b8b3ec9d..1d1e8a3a9844d 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -17,8 +17,8 @@ #include -#include -#include +#include +#include #include #include @@ -33,7 +33,7 @@ namespace motion_planning using diagnostic_msgs::msg::DiagnosticStatus; using geometry_msgs::msg::Pose; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; struct StopPoint { @@ -256,7 +256,7 @@ struct PlannerData pcl::PointXYZ lateral_nearest_slow_down_point; - autoware_auto_perception_msgs::msg::Shape slow_down_object_shape{}; + autoware_perception_msgs::msg::Shape slow_down_object_shape{}; rclcpp::Time nearest_collision_point_time{}; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index d4a7d92d8f161..b43e51873c09e 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -45,15 +45,15 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using std_msgs::msg::Header; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using PointVariant = std::variant; std::optional> calcFeasibleMarginAndVelocity( @@ -111,10 +111,10 @@ void getLateralNearestPointForPredictedObject( Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info); Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertBoundingBoxObjectToGeometryPolygon( const Pose & current_pose, const double & base_to_front, const double & base_to_rear, @@ -145,7 +145,7 @@ std::optional getObstacleFromUuid( bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos); -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape); rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr); diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index 1a6f8433875ce..fcf09b96c1345 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -21,8 +21,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager diagnostic_msgs motion_utils diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index 965d52a882568..2931da5830a60 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -198,7 +198,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header) { @@ -306,7 +306,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( calcUpperVelocity(col_point_distance, point_velocity, current_velocity); pub_debug_->publish(debug_values_); - if (target_object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (target_object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // if the target object is obstacle return stop true RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), @@ -399,7 +399,7 @@ double AdaptiveCruiseController::calcTrajYaw( } std::optional AdaptiveCruiseController::estimatePointVelocityFromObject( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const double traj_yaw, const pcl::PointXYZ & nearest_collision_point) { geometry_msgs::msg::Point nearest_collision_p_ros; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 8f57b3e0f051a..31c044c93af22 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -40,7 +40,7 @@ namespace motion_planning { -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; @@ -628,7 +628,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_slow_down_range; bool found_slow_down_object = false; Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -636,7 +636,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( slow_down_param_.pedestrian_lateral_margin); found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range, vehicle_info, slow_down_param_.vehicle_lateral_margin); @@ -647,7 +647,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range, vehicle_info, slow_down_param_.unknown_lateral_margin); @@ -722,15 +722,15 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_slow_down_vehicle_polygon; const auto & obj = filtered_objects.objects.at(nearest_slow_down_object_index); - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.pedestrian_lateral_margin); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.vehicle_lateral_margin); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.unknown_lateral_margin); @@ -763,7 +763,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_collision_polygon; bool found_collision_object = false; Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); @@ -772,7 +772,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.pedestrian_lateral_margin); found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; object_polygon = convertBoundingBoxObjectToGeometryPolygon( @@ -782,7 +782,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.vehicle_lateral_margin); found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -846,7 +846,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d object_polygon{}; Polygon2d one_step_move_vehicle_polygon; bool found_collision_object = false; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); @@ -855,7 +855,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.pedestrian_lateral_margin); found_collision_object = bg::intersects(one_step_move_vehicle_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; object_polygon = convertBoundingBoxObjectToGeometryPolygon( @@ -865,7 +865,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.vehicle_lateral_margin); found_collision_object = bg::intersects(one_step_move_vehicle_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -1128,7 +1128,7 @@ void ObstacleStopPlannerNode::insertVelocity( if (node_param_.use_predicted_objects) { if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + autoware_perception_msgs::msg::Shape::CYLINDER) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * @@ -1136,7 +1136,7 @@ void ObstacleStopPlannerNode::insertVelocity( slow_down_param_.pedestrian_lateral_margin; } else if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * @@ -1144,7 +1144,7 @@ void ObstacleStopPlannerNode::insertVelocity( slow_down_param_.vehicle_lateral_margin; } else if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::POLYGON) { + autoware_perception_msgs::msg::Shape::POLYGON) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 31fb843c234d9..3e2ecca8ec4d7 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -32,8 +32,8 @@ namespace motion_planning { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -632,7 +632,7 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( } Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; @@ -655,7 +655,7 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; tf2::Transform tf_map2obj; @@ -698,13 +698,13 @@ bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & ob return base_pose_vec.dot(obstacle_vec) >= 0; } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index 36aa35bb31156..9afe338cc17e0 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -146,22 +146,22 @@ For example a value of `1` means all trajectory points will be evaluated while a ### Inputs -| Name | Type | Description | -| ----------------------------- | ------------------------------------------------ | -------------------------------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | -| `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | -| `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud containing only obstacle points | -| `~/input/dynamic_obstacles` | `autoware_auto_perception_msgs/PredictedObjects` | Dynamic objects | -| `~/input/odometry` | `nav_msgs/Odometry` | Odometry used to retrieve the current ego velocity | -| `~/input/map` | `autoware_auto_mapping_msgs/HADMapBin` | Vector map used to retrieve static obstacles | +| Name | Type | Description | +| ----------------------------- | ------------------------------------------- | -------------------------------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | +| `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud containing only obstacle points | +| `~/input/dynamic_obstacles` | `autoware_perception_msgs/PredictedObjects` | Dynamic objects | +| `~/input/odometry` | `nav_msgs/Odometry` | Odometry used to retrieve the current ego velocity | +| `~/input/map` | `autoware_map_msgs/LaneletMapBin` | Vector map used to retrieve static obstacles | ### Outputs -| Name | Type | Description | -| ------------------------------- | ---------------------------------------- | -------------------------------------------------------- | -| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Trajectory with adjusted velocities | -| `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | -| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | +| Name | Type | Description | +| ------------------------------- | ----------------------------------- | -------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Trajectory with adjusted velocities | +| `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | +| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | ## Parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp index 3f5f8cd814ea3..d3074e3fb1b45 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp @@ -18,7 +18,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -#include +#include #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp index 4d89d4b440c21..0beced62bd7d6 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp @@ -19,7 +19,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -#include +#include #include #include @@ -63,8 +63,8 @@ void calculateSteeringAngles(Trajectory & trajectory, const Float wheel_base); /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, - const Float buffer, const Float min_vel); + const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, + const Float min_vel); /// @brief create footprint polygons from projection lines /// @details A footprint is create for each group of lines. Each group of lines is assumed to share diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 63eec0f2cf378..f0b9d34413c9a 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -25,9 +25,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -69,7 +69,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node sub_pointcloud_; //!< @brief subscriber for obstacle pointcloud rclcpp::Subscription::SharedPtr sub_odom_; //!< @brief subscriber for the current velocity - rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr map_sub_; // cached inputs PredictedObjects::ConstSharedPtr dynamic_obstacles_ptr_; diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp index 8620c254968fe..618437edc981a 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include @@ -163,7 +163,7 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp index e44011e1f0ee3..6bd1b39b02f73 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp @@ -17,17 +17,17 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include -#include -#include +#include +#include +#include #include #include namespace obstacle_velocity_limiter { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using nav_msgs::msg::OccupancyGrid; using PointCloud = sensor_msgs::msg::PointCloud2; using Float = decltype(TrajectoryPoint::longitudinal_velocity_mps); diff --git a/planning/obstacle_velocity_limiter/package.xml b/planning/obstacle_velocity_limiter/package.xml index 8c9c89094f5f0..49e424bfbc9ef 100644 --- a/planning/obstacle_velocity_limiter/package.xml +++ b/planning/obstacle_velocity_limiter/package.xml @@ -11,8 +11,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager eigen grid_map_msgs diff --git a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py index c9255dae679c0..2e56e8034555a 100755 --- a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py +++ b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import Trajectory import matplotlib.pyplot as plt from nav_msgs.msg import Odometry import numpy as np @@ -31,7 +31,7 @@ def __init__(self): self.sub_original_traj = self.create_subscription( Trajectory, - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory", + "/planning/scenario_planning/lane_driving/motion_planning/path_optimizer/trajectory", self.plotOriginalTrajectory, 1, ) diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp index ac1b4bde65211..7c050b078c3aa 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp @@ -35,8 +35,8 @@ Float calculateSafeVelocity( } multi_polygon_t createPolygonMasks( - const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, - const Float buffer, const Float min_vel) + const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, + const Float min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); } diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index ea08c2f7cf552..18cf534da8c7e 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -57,9 +57,9 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio sub_odom_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, [this](const nav_msgs::msg::Odometry::ConstSharedPtr msg) { current_odometry_ptr_ = msg; }); - map_sub_ = create_subscription( + map_sub_ = create_subscription( "~/input/map", rclcpp::QoS{1}.transient_local(), - [this](const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { + [this](const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); static_map_obstacles_ = extractStaticObstacles(*lanelet_map_ptr_, obstacle_params_.static_map_tags); diff --git a/planning/obstacle_velocity_limiter/src/obstacles.cpp b/planning/obstacle_velocity_limiter/src/obstacles.cpp index 603e573e68f17..5d4c1d29665df 100644 --- a/planning/obstacle_velocity_limiter/src/obstacles.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacles.cpp @@ -59,7 +59,7 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity) { multi_polygon_t polygons; diff --git a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp index c5e69e5fd8fb9..d4f44e7a6a775 100644 --- a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp +++ b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp @@ -17,8 +17,8 @@ #include "obstacle_velocity_limiter/types.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include -#include +#include +#include #include #include @@ -384,8 +384,8 @@ TEST(TestCollisionDistance, arcDistance) TEST(TestCollisionDistance, createObjPolygons) { - using autoware_auto_perception_msgs::msg::PredictedObject; - using autoware_auto_perception_msgs::msg::PredictedObjects; + using autoware_perception_msgs::msg::PredictedObject; + using autoware_perception_msgs::msg::PredictedObjects; using obstacle_velocity_limiter::createObjectPolygons; PredictedObjects objects; diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index e0bdb326adb33..bc566fdfb96bf 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -22,6 +22,7 @@ #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include @@ -72,9 +73,6 @@ class ElasticBandSmoother : public rclcpp::Node CommonParam common_param_{}; EgoNearestParam ego_nearest_param_{}; - // variables for subscribers - Odometry::ConstSharedPtr ego_state_ptr_; - // variables for previous information std::shared_ptr> prev_optimized_traj_points_ptr_; @@ -84,7 +82,7 @@ class ElasticBandSmoother : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + tier4_autoware_utils::InterProcessPollingSubscriber odom_sub_{this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; @@ -104,7 +102,8 @@ class ElasticBandSmoother : public rclcpp::Node void resetPreviousData(); // main functions - bool isDataReady(const Path & path, rclcpp::Clock clock) const; + bool isDataReady( + const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const; void applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points, diff --git a/planning/path_smoother/include/path_smoother/type_alias.hpp b/planning/path_smoother/include/path_smoother/type_alias.hpp index c8f6d6da98dcd..21cbcf8e7eefc 100644 --- a/planning/path_smoother/include/path_smoother/type_alias.hpp +++ b/planning/path_smoother/include/path_smoother/type_alias.hpp @@ -15,10 +15,10 @@ #ifndef PATH_SMOOTHER__TYPE_ALIAS_HPP_ #define PATH_SMOOTHER__TYPE_ALIAS_HPP_ -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" @@ -30,10 +30,10 @@ namespace path_smoother // std_msgs using std_msgs::msg::Header; // planning -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; // navigation using nav_msgs::msg::Odometry; // debug diff --git a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp index 067f5d2c57cc4..55a7829a5bea3 100644 --- a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp +++ b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp @@ -24,8 +24,8 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "boost/optional/optional_fwd.hpp" diff --git a/planning/path_smoother/package.xml b/planning/path_smoother/package.xml index 0cba14254e492..a0e83fdf8a091 100644 --- a/planning/path_smoother/package.xml +++ b/planning/path_smoother/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 6fb732309da4b..75300286ac9dc 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -79,8 +79,6 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option // interface subscriber path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ElasticBandSmoother::onPath, this, std::placeholders::_1)); - odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -156,7 +154,8 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) time_keeper_ptr_->tic(__func__); // check if data is ready and valid - if (!isDataReady(*path_ptr, *get_clock())) { + const auto ego_state_ptr = odom_sub_.takeData(); + if (!isDataReady(*path_ptr, ego_state_ptr, *get_clock())) { return; } @@ -181,7 +180,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) // 1. calculate trajectory with Elastic Band // 1.a check if replan (= optimization) is required PlannerData planner_data( - input_traj_points, ego_state_ptr_->pose.pose, ego_state_ptr_->twist.twist.linear.x); + input_traj_points, ego_state_ptr->pose.pose, ego_state_ptr->twist.twist.linear.x); const bool is_replan_required = [&]() { if (replan_checker_ptr_->isResetRequired(planner_data)) { // NOTE: always replan when resetting previous optimization @@ -195,7 +194,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) replan_checker_ptr_->updateData(planner_data, is_replan_required, now()); time_keeper_ptr_->tic(__func__); auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory( - input_traj_points, ego_state_ptr_->pose.pose) + input_traj_points, ego_state_ptr->pose.pose) : *prev_optimized_traj_points_ptr_; time_keeper_ptr_->toc(__func__, " "); @@ -203,7 +202,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) std::make_shared>(smoothed_traj_points); // 2. update velocity - applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose); + applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr->pose.pose); // 3. extend trajectory to connect the optimized trajectory and the following path smoothly auto full_traj_points = extendTrajectory(input_traj_points, smoothed_traj_points); @@ -230,9 +229,10 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp); } -bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) const +bool ElasticBandSmoother::isDataReady( + const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const { - if (!ego_state_ptr_) { + if (!ego_state_ptr) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist."); return false; } diff --git a/planning/path_smoother/src/utils/trajectory_utils.cpp b/planning/path_smoother/src/utils/trajectory_utils.cpp index 6409c61700f34..b4d0950a593d5 100644 --- a/planning/path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/path_smoother/src/utils/trajectory_utils.cpp @@ -18,8 +18,8 @@ #include "motion_utils/trajectory/conversion.hpp" #include "path_smoother/utils/geometry_utils.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/planning_test_utils/CMakeLists.txt b/planning/planning_test_utils/CMakeLists.txt index 56e985c7fa2b7..ce746352bb89c 100644 --- a/planning/planning_test_utils/CMakeLists.txt +++ b/planning/planning_test_utils/CMakeLists.txt @@ -4,6 +4,9 @@ project(planning_test_utils) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(planning_test_utils SHARED + src/planning_test_utils.cpp) + ament_auto_add_library(mock_data_parser SHARED src/mock_data_parser.cpp) diff --git a/planning/planning_test_utils/README.md b/planning/planning_test_utils/README.md index b72625be999dd..c064e5791a487 100644 --- a/planning/planning_test_utils/README.md +++ b/planning/planning_test_utils/README.md @@ -1,92 +1,40 @@ -# Autoware Planning Test Manager +# Test Utils ## Background -In each node of the planning module, when exceptional input, such as unusual routes or significantly deviated ego-position, is given, the node may not be prepared for such input and could crash. As a result, debugging node crashes can be time-consuming. For example, if an empty trajectory is given as input and it was not anticipated during implementation, the node might crash due to the unaddressed exceptional input when changes are merged, during scenario testing or while the system is running on an actual vehicle. +Several Autoware's components and modules have already adopted unit testing, so a common library to ease the process of writing unit tests is necessary. ## Purpose -The purpose is to provide a utility for implementing tests to ensure that node operates correctly when receiving exceptional input. By utilizing this utility and implementing tests for exceptional input, the purpose is to reduce bugs that are only discovered when actually running the system, by requiring measures for exceptional input before merging PRs. +The objective of the `test_utils` is to develop a unit testing library for the Autoware components. This library will include -## Features +- commonly used functions +- input/mock data parser +- maps for testing +- common routes and mock data for testing. -### Confirmation of normal operation +## Available Maps -For the test target node, confirm that the node operates correctly and publishes the required messages for subsequent nodes. To do this, test_node publish the necessary messages and confirm that the node's output is being published. +The following maps are available [here](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/planning_test_utils/test_map) -### Robustness confirmation for special inputs +### Common -After confirming normal operation, ensure that the test target node does not crash when given exceptional input. To do this, provide exceptional input from the test_node and confirm that the node does not crash. +The common map contains multiple types of usable inputs, including shoulder lanes, intersections, and some regulatory elements. The common map is named `lanelet2_map.osm` in the folder. -(WIP) +![common](./images/common.png) -## Usage +### 2 km Straight -```cpp +The 2 km straight lanelet map consists of two lanes that run in the same direction. The map is named `2km_test.osm`. -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) -{ - rclcpp::init(0, nullptr); +![two_km](./images/2km-test.png) - // instantiate test_manager with PlanningInterfaceTestManager type - auto test_manager = std::make_shared(); +The following illustrates the design of the map. - // get package directories for necessary configuration files - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto target_node_dir = - ament_index_cpp::get_package_share_directory("target_node"); +![straight_diagram](./images/2km-test.svg) - // set arguments to get the config file - 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"}); +## Example use cases - // instantiate the TargetNode with node_options - auto test_target_node = std::make_shared(node_options); +### Autoware Planning Test Manager - // publish the necessary topics from test_manager second argument is topic name - test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); - test_manager->publishMaxVelocity( - test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); - - // set scenario_selector's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("input/parking/trajectory"); - - // test with normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); - - // make sure target_node is running - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - // test with trajectory input with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); - - // shutdown ROS context - rclcpp::shutdown(); -} -``` - -## Implemented tests - -| Node | Test name | exceptional input | output | Exceptional input pattern | -| -------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | -| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| motion_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 | -| obstacle_avoidance_planner | 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 - -During test execution, when launching a node, parameters are loaded from the parameter file within each package. Therefore, when adding parameters, it is necessary to add the required parameters to the parameter file in the target node package. This is to prevent the node from being unable to launch if there are missing parameters when retrieving them from the parameter file during node launch. - -## Future extensions / Unimplemented parts - -(WIP) +The goal of the [Autoware Planning Test Manager](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_planning_test_manager/) is to test planning module nodes. The `PlanningInterfaceTestManager` class ([source code](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp)) creates wrapper functions based on the `test_utils` functions. diff --git a/planning/planning_test_utils/images/2km-test.png b/planning/planning_test_utils/images/2km-test.png new file mode 100644 index 0000000000000..297dc5a43865e Binary files /dev/null and b/planning/planning_test_utils/images/2km-test.png differ diff --git a/planning/planning_test_utils/images/2km-test.svg b/planning/planning_test_utils/images/2km-test.svg new file mode 100644 index 0000000000000..26632cdde9118 --- /dev/null +++ b/planning/planning_test_utils/images/2km-test.svg @@ -0,0 +1,505 @@ + + + + + + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 3.5 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2000.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 1.75 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (0.0, 0.0) +
+
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ 1000.0 meter +
+
+
+ + + + + + + + + + + + + + + + + +
+
+
+ 7.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + +
+
+
+ pose +
+ (1000.0, -3.5) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (1000.0, 3.5) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (-1000.0, 3.5) +
+
+
+
+
+ + + + + + + diff --git a/planning/planning_test_utils/images/common.png b/planning/planning_test_utils/images/common.png new file mode 100644 index 0000000000000..340437b53c0f5 Binary files /dev/null and b/planning/planning_test_utils/images/common.png differ diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp index 6af1ce165d7ab..9d787070a9dc5 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp @@ -26,19 +26,19 @@ #include #include -#include -#include -#include -#include +#include #include #include #include +#include #include #include #include #include #include #include +#include +#include #include #include @@ -57,14 +57,14 @@ namespace test_utils { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using RouteSections = std::vector; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -79,20 +79,248 @@ using tier4_autoware_utils::createQuaternionFromRPY; using tier4_planning_msgs::msg::Scenario; using unique_identifier_msgs::msg::UUID; +/** + * @brief Creates a Pose message with the specified position and orientation. + * + * This function initializes a geometry_msgs::msg::Pose message with the + * given position (x, y, z) and orientation (roll, pitch, yaw). + * + * @param x The x-coordinate of the position. + * @param y The y-coordinate of the position. + * @param z The z-coordinate of the position. + * @param roll The roll component of the orientation (in radians). + * @param pitch The pitch component of the orientation (in radians). + * @param yaw The yaw component of the orientation (in radians). + * @return A geometry_msgs::msg::Pose message with the specified position + * and orientation. + */ geometry_msgs::msg::Pose createPose( - double x, double y, double z, double roll, double pitch, double yaw) -{ - geometry_msgs::msg::Pose p; - p.position = createPoint(x, y, z); - p.orientation = createQuaternionFromRPY(roll, pitch, yaw); - return p; -} - -geometry_msgs::msg::Pose createPose(const std::array & pose3d) -{ - return createPose(pose3d[0], pose3d[1], pose3d[2], 0.0, 0.0, pose3d[3]); -} - + double x, double y, double z, double roll, double pitch, double yaw); + +/** + * @brief Creates a Pose message from a 4-element array representing a 3D pose. + * + * This function initializes a geometry_msgs::msg::Pose message using a + * std::array of four doubles, where the first three elements represent the + * position (x, y, z) and the fourth element represents the yaw orientation. + * The roll and pitch orientations are assumed to be zero. + * + * @param pose3d A std::array of four doubles representing the 3D pose. The + * first element is the x-coordinate, the second is the y-coordinate, the + * third is the z-coordinate, and the fourth is the yaw orientation. + * @return A geometry_msgs::msg::Pose message with the specified position + * and yaw orientation, with roll and pitch set to zero. + */ +geometry_msgs::msg::Pose createPose(const std::array & pose3d); + +/** + * @brief Creates a LaneletSegment with the specified ID. + * + * Initializes a LaneletSegment containing a single LaneletPrimitive with the + * given ID and a primitive type of "lane". + * + * @param id The ID for the LaneletPrimitive and preferred primitive. + * @return A LaneletSegment with the specified ID. + */ +LaneletSegment createLaneletSegment(int id); + +/** + * @brief Loads a Lanelet map from a specified file. + * + * This function loads a Lanelet2 map using the given filename. It uses the MGRS + * projector and checks for any errors during the loading process. If errors + * are found, they are logged, and the function returns nullptr. + * + * @param lanelet2_filename The filename of the Lanelet2 map to load. + * @return A LaneletMapPtr to the loaded map, or nullptr if there were errors. + */ +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename); + +/** + * @brief Converts a Lanelet map to a LaneletMapBin message. + * + * This function converts a given Lanelet map to a LaneletMapBin message. It also + * parses the format and map versions from the specified filename and includes + * them in the message. The timestamp for the message is set to the provided time. + * + * @param map The Lanelet map to convert. + * @param lanelet2_filename The filename of the Lanelet2 map, used to parse format and map versions. + * @param now The current time to set in the message header. + * @return A LaneletMapBin message containing the converted map data. + */ +LaneletMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, + const rclcpp::Time & now); + +/** + * @brief Creates a normal Lanelet route with predefined start and goal poses. + * + * This function initializes a LaneletRoute with a predefined start and goal pose. + * + * @return A LaneletRoute with the specified start and goal poses. + */ +LaneletRoute makeNormalRoute(); + +/** + * @brief Creates an OccupancyGrid message with the specified dimensions and resolution. + * + * This function initializes an OccupancyGrid message with given width, height, and resolution. + * All cells in the grid are initialized to zero. + * + * @param width The width of the occupancy grid. + * @param height The height of the occupancy grid. + * @param resolution The resolution of the occupancy grid. + * @return An OccupancyGrid message with the specified dimensions and resolution. + */ +OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2); + +/** + * @brief Creates a LaneletMapBin message from a Lanelet map file. + * + * This function loads a Lanelet map from the given file, overwrites the + * centerline with the specified resolution, and converts the map to a LaneletMapBin message. + * + * @param absolute_path The absolute path to the Lanelet2 map file. + * @param center_line_resolution The resolution for the centerline. + * @return A LaneletMapBin message containing the map data. + */ +LaneletMapBin make_map_bin_msg( + const std::string & absolute_path, const double center_line_resolution = 5.0); + +/** + * @brief Creates a LaneletMapBin message using a predefined Lanelet2 map file. + * + * This function loads a lanelet2_map.osm from the test_map folder in the + * planning_test_utils package, overwrites the centerline with a resolution of 5.0, + * and converts the map to a LaneletMapBin message. + * + * @return A LaneletMapBin message containing the map data. + */ +LaneletMapBin makeMapBinMsg(); + +/** + * @brief Creates an Odometry message with a specified shift. + * + * This function initializes an Odometry message with a pose shifted by the given amount at y + * direction. x pose, z pose, and yaw angle remains zero. + * + * @param shift The amount by which to shift the pose. + * @return An Odometry message with the specified shift. + */ +Odometry makeOdometry(const double shift = 0.0); + +/** + * @brief Creates an initial Odometry message with a specified shift. + * + * This function initializes an Odometry message with a pose shifted by the given amount, + * accounting for a specific yaw angle. + * + * @param shift The amount by which to shift the pose. + * @return An Odometry message with the specified shift. + */ +Odometry makeInitialPose(const double shift = 0.0); + +/** + * @brief Creates a TFMessage with the specified frame IDs. + * + * This function initializes a TFMessage with a transform containing the given frame IDs. + * The transform includes a specific translation and rotation. + * + * @param target_node The node to use for obtaining the current time. + * @param frame_id The ID of the parent frame. + * @param child_frame_id The ID of the child frame. + * @return A TFMessage containing the transform. + */ +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string && frame_id = "", + std::string && child_frame_id = ""); + +/** + * @brief Creates a Scenario message with the specified scenario. + * + * This function initializes a Scenario message with the current scenario and a list of activating + * scenarios. + * + * @param scenario The name of the current scenario. + * @return A Scenario message with the specified scenario. + */ +Scenario makeScenarioMsg(const std::string & scenario); + +/** + * @brief Combines two sets of consecutive RouteSections. + * + * This function combines two sets of RouteSections, removing the overlapping end section from the + * first set. + * + * @param route_sections1 The first set of RouteSections. + * @param route_sections2 The second set of RouteSections. + * @return A combined set of RouteSections. + */ +RouteSections combineConsecutiveRouteSections( + const RouteSections & route_sections1, const RouteSections & route_sections2); + +/** + * @brief Creates a predefined behavior Lanelet route. + * + * This function initializes a LaneletRoute with predefined start and goal poses, + * a list of lanelet segment IDs, and a fixed UUID. + * this is for the test lanelet2_map.osm + * file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 + * + * @return A LaneletRoute with the specified attributes. + */ +LaneletRoute makeBehaviorNormalRoute(); + +/** + * @brief Spins multiple ROS nodes a specified number of times. + * + * This function spins the given test and target nodes for the specified number of iterations. + * + * @param test_node The test node to spin. + * @param target_node The target node to spin. + * @param repeat_count The number of times to spin the nodes. + */ +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, + const int repeat_count = 1); + +/** + * @brief Updates node options with parameter files. + * + * This function updates the given node options to include the specified parameter files. + * + * @param node_options The node options to update. + * @param params_files A vector of parameter file paths to add to the node options. + */ +void updateNodeOptions( + rclcpp::NodeOptions & node_options, const std::vector & params_files); + +/** + * @brief Loads a PathWithLaneId message from a YAML file. + * + * This function loads a PathWithLaneId message from a YAML file located in the + * planning_test_utils package. + * + * @return A PathWithLaneId message containing the loaded data. + */ +PathWithLaneId loadPathWithLaneIdInYaml(); + +/** + * @brief Generates a trajectory with specified parameters. + * + * This function generates a trajectory of type T with a given number of points, + * point interval, velocity, initial theta, delta theta, and optionally an + * overlapping point index. + * + * @tparam T The type of the trajectory. + * @param num_points The number of points in the trajectory. + * @param point_interval The distance between consecutive points. + * @param velocity The longitudinal velocity for each point. + * @param init_theta The initial theta angle. + * @param delta_theta The change in theta per point. + * @param overlapping_point_index The index of the point to overlap (default is max size_t value). + * @return A trajectory of type T with the specified parameters. + */ template T generateTrajectory( const size_t num_points, const double point_interval, const double velocity = 0.0, @@ -121,204 +349,24 @@ T generateTrajectory( return traj; } -LaneletSegment createLaneletSegment(int id) -{ - LaneletPrimitive primitive; - primitive.id = id; - primitive.primitive_type = "lane"; - LaneletSegment segment; - segment.preferred_primitive.id = id; - segment.primitives.push_back(primitive); - return segment; -} - -lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) -{ - lanelet::ErrorMessages errors{}; - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; - } - - for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); - } - return nullptr; -} - -HADMapBin convertToMapBinMsg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) -{ - std::string format_version{}, map_version{}; - lanelet::io_handlers::AutowareOsmParser::parseVersions( - lanelet2_filename, &format_version, &map_version); - - HADMapBin map_bin_msg; - map_bin_msg.header.stamp = now; - map_bin_msg.header.frame_id = "map"; - map_bin_msg.format_version = format_version; - map_bin_msg.map_version = map_version; - lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); - - return map_bin_msg; -} - -LaneletRoute makeNormalRoute() -{ - const std::array start_pose{5.5, 4., 0., M_PI_2}; - const std::array goal_pose{8.0, 26.3, 0, 0}; - LaneletRoute route; - route.header.frame_id = "map"; - route.start_pose = createPose(start_pose); - route.goal_pose = createPose(goal_pose); - return route; -} - -OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2) -{ - nav_msgs::msg::OccupancyGrid costmap_msg; - - // create info - costmap_msg.header.frame_id = "map"; - costmap_msg.info.width = width; - costmap_msg.info.height = height; - costmap_msg.info.resolution = resolution; - - // create data - const size_t n_elem = width * height; - for (size_t i = 0; i < n_elem; ++i) { - costmap_msg.data.push_back(0.0); - } - return costmap_msg; -} - -HADMapBin make_map_bin_msg( - const std::string & absolute_path, const double center_line_resolution = 5.0) -{ - const auto map = loadMap(absolute_path); - if (!map) { - return autoware_auto_mapping_msgs::msg::HADMapBin_>{}; - } - - // overwrite centerline - lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); - - // create map bin msg - const auto map_bin_msg = - convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now()); - return map_bin_msg; -} - -HADMapBin makeMapBinMsg() -{ - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; - double center_line_resolution = 5.0; - - return make_map_bin_msg(lanelet2_path, center_line_resolution); -} - -Odometry makeOdometry(const double shift = 0.0) -{ - Odometry odometry; - const std::array start_pose{0.0, shift, 0.0, 0.0}; - odometry.pose.pose = createPose(start_pose); - odometry.header.frame_id = "map"; - return odometry; -} - -Odometry makeInitialPose(const double shift = 0.0) -{ - Odometry current_odometry; - const auto yaw = 0.9724497591854532; - const auto shift_x = shift * std::sin(yaw); - const auto shift_y = shift * std::cos(yaw); - const std::array start_pose{ - 3722.16015625 + shift_x, 73723.515625 + shift_y, 0.233112560494183, yaw}; - current_odometry.pose.pose = test_utils::createPose(start_pose); - current_odometry.header.frame_id = "map"; - return current_odometry; -} - -TFMessage makeTFMsg( - rclcpp::Node::SharedPtr target_node, std::string frame_id = "", std::string child_frame_id = "") -{ - TFMessage tf_msg; - geometry_msgs::msg::Quaternion quaternion; - quaternion.x = 0.; - quaternion.y = 0.; - quaternion.z = 0.23311256049418302; - quaternion.w = 0.9724497591854532; - - TransformStamped tf; - tf.header.stamp = target_node->get_clock()->now(); - tf.header.frame_id = frame_id; - tf.child_frame_id = child_frame_id; - tf.transform.translation.x = 3722.16015625; - tf.transform.translation.y = 73723.515625; - tf.transform.translation.z = 0; - tf.transform.rotation = quaternion; - - tf_msg.transforms.emplace_back(std::move(tf)); - return tf_msg; -} - -Scenario makeScenarioMsg(const std::string scenario) -{ - Scenario scenario_msg; - scenario_msg.current_scenario = scenario; - scenario_msg.activating_scenarios = {scenario}; - return scenario_msg; -} - -RouteSections combineConsecutiveRouteSections( - const RouteSections & route_sections1, const RouteSections & route_sections2) -{ - RouteSections route_sections; - route_sections.reserve(route_sections1.size() + route_sections2.size()); - if (!route_sections1.empty()) { - // remove end route section because it is overlapping with first one in next route_section - route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1); - } - if (!route_sections2.empty()) { - route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end()); - } - return route_sections; -} - -// this is for the test lanelet2_map.osm -// file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 -LaneletRoute makeBehaviorNormalRoute() -{ - LaneletRoute route; - route.header.frame_id = "map"; - route.start_pose = - createPose({3722.16015625, 73723.515625, 0.233112560494183, 0.9724497591854532}); - route.goal_pose = - createPose({3778.362060546875, 73721.2734375, -0.5107480274693206, 0.8597304533609347}); - - std::vector primitive_ids = {9102, 9178, 54, 112}; - for (int id : primitive_ids) { - route.segments.push_back(createLaneletSegment(id)); - } - - std::array uuid_bytes{210, 87, 16, 126, 98, 151, 58, 28, - 252, 221, 230, 92, 122, 170, 46, 6}; - route.uuid.uuid = uuid_bytes; - - route.allow_modification = false; - return route; -} - +/** + * @brief Creates a publisher with appropriate QoS settings. + * + * This function creates a publisher for a given topic name and message type with appropriate + * QoS settings, depending on the message type. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param topic_name The name of the topic to publish to. + * @param publisher A reference to the publisher to be created. + */ template void createPublisherWithQoS( rclcpp::Node::SharedPtr test_node, std::string topic_name, std::shared_ptr> & publisher) { if constexpr ( - std::is_same_v || std::is_same_v || + std::is_same_v || std::is_same_v || std::is_same_v) { rclcpp::QoS qos(rclcpp::KeepLast(1)); qos.reliable(); @@ -329,6 +377,16 @@ void createPublisherWithQoS( } } +/** + * @brief Sets up a publisher for a given topic. + * + * This function sets up a publisher for a given topic using createPublisherWithQoS. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param topic_name The name of the topic to publish to. + * @param publisher A reference to the publisher to be set. + */ template void setPublisher( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -337,6 +395,18 @@ void setPublisher( createPublisherWithQoS(test_node, topic_name, publisher); } +/** + * @brief Creates a subscription with appropriate QoS settings. + * + * This function creates a subscription for a given topic name and message type with appropriate + * QoS settings, depending on the message type. + * + * @tparam T The type of the message to subscribe to. + * @param test_node The node to create the subscription on. + * @param topic_name The name of the topic to subscribe to. + * @param callback The callback function to call when a message is received. + * @param subscriber A reference to the subscription to be created. + */ template void createSubscription( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -350,6 +420,17 @@ void createSubscription( } } +/** + * @brief Sets up a subscriber for a given topic. + * + * This function sets up a subscriber for a given topic using createSubscription. + * + * @tparam T The type of the message to subscribe to. + * @param test_node The node to create the subscription on. + * @param topic_name The name of the topic to subscribe to. + * @param subscriber A reference to the subscription to be set. + * @param count A reference to a counter that increments on message receipt. + */ template void setSubscriber( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -359,32 +440,32 @@ void setSubscriber( test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber); } -void spinSomeNodes( - rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, - const int repeat_count = 1) -{ - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(test_node); - executor.add_node(target_node); - for (int i = 0; i < repeat_count; i++) { - executor.spin_some(std::chrono::milliseconds(100)); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } -} - +/** + * @brief Publishes data to a target node. + * + * This function publishes data to a target node on a given topic, ensuring that the topic has + * subscribers and retrying a specified number of times. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param target_node The target node to publish the message to. + * @param topic_name The name of the topic to publish to. + * @param publisher A shared pointer to the publisher. + * @param data The data to publish. + * @param repeat_count The number of times to retry publishing. + */ template void publishToTargetNode( rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, std::string topic_name, typename rclcpp::Publisher::SharedPtr publisher, T data, const int repeat_count = 3) { if (topic_name.empty()) { - int status; + int status = 1; char * demangled_name = abi::__cxa_demangle(typeid(data).name(), nullptr, nullptr, &status); if (status == 0) { throw std::runtime_error(std::string("Topic name for ") + demangled_name + " is empty"); - } else { - throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); } + throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); } test_utils::setPublisher(test_node, topic_name, publisher); @@ -396,83 +477,6 @@ void publishToTargetNode( test_utils::spinSomeNodes(test_node, target_node, repeat_count); } -void updateNodeOptions( - rclcpp::NodeOptions & node_options, const std::vector & params_files) -{ - std::vector arguments; - arguments.push_back("--ros-args"); - arguments.push_back("--params-file"); - for (const auto & param_file : params_files) { - arguments.push_back(param_file.c_str()); - arguments.push_back("--params-file"); - } - arguments.pop_back(); - - node_options.arguments(std::vector{arguments.begin(), arguments.end()}); -} - -PathWithLaneId loadPathWithLaneIdInYaml() -{ - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto yaml_path = planning_test_utils_dir + "/config/path_with_lane_id_data.yaml"; - YAML::Node yaml_node = YAML::LoadFile(yaml_path); - PathWithLaneId path_msg; - - // Convert YAML data to PathWithLaneId message - // Fill the header - path_msg.header.stamp.sec = yaml_node["header"]["stamp"]["sec"].as(); - path_msg.header.stamp.nanosec = yaml_node["header"]["stamp"]["nanosec"].as(); - path_msg.header.frame_id = yaml_node["header"]["frame_id"].as(); - - // Fill the points - for (const auto & point_node : yaml_node["points"]) { - PathPointWithLaneId point; - // Fill the PathPoint data - point.point.pose.position.x = point_node["point"]["pose"]["position"]["x"].as(); - point.point.pose.position.y = point_node["point"]["pose"]["position"]["y"].as(); - point.point.pose.position.z = point_node["point"]["pose"]["position"]["z"].as(); - point.point.pose.orientation.x = point_node["point"]["pose"]["orientation"]["x"].as(); - point.point.pose.orientation.y = point_node["point"]["pose"]["orientation"]["y"].as(); - point.point.pose.orientation.z = point_node["point"]["pose"]["orientation"]["z"].as(); - point.point.pose.orientation.w = point_node["point"]["pose"]["orientation"]["w"].as(); - point.point.longitudinal_velocity_mps = - point_node["point"]["longitudinal_velocity_mps"].as(); - point.point.lateral_velocity_mps = point_node["point"]["lateral_velocity_mps"].as(); - point.point.heading_rate_rps = point_node["point"]["heading_rate_rps"].as(); - point.point.is_final = point_node["point"]["is_final"].as(); - // Fill the lane_ids - for (const auto & lane_id_node : point_node["lane_ids"]) { - point.lane_ids.push_back(lane_id_node.as()); - } - - path_msg.points.push_back(point); - } - - // Fill the left_bound - for (const auto & point_node : yaml_node["left_bound"]) { - Point point; - // Fill the Point data (left_bound) - point.x = point_node["x"].as(); - point.y = point_node["y"].as(); - point.z = point_node["z"].as(); - - path_msg.left_bound.push_back(point); - } - - // Fill the right_bound - for (const auto & point_node : yaml_node["right_bound"]) { - Point point; - // Fill the Point data - point.x = point_node["x"].as(); - point.y = point_node["y"].as(); - point.z = point_node["z"].as(); - - path_msg.right_bound.push_back(point); - } - return path_msg; -} - } // namespace test_utils #endif // PLANNING_TEST_UTILS__PLANNING_TEST_UTILS_HPP_ diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index b236a64f48da0..8f3c3259c33d0 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -15,12 +15,11 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils lanelet2_extension diff --git a/planning/planning_test_utils/src/planning_test_utils.cpp b/planning/planning_test_utils/src/planning_test_utils.cpp new file mode 100644 index 0000000000000..3ba97552ef250 --- /dev/null +++ b/planning/planning_test_utils/src/planning_test_utils.cpp @@ -0,0 +1,314 @@ +// 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 "planning_test_utils/planning_test_utils.hpp" +namespace test_utils +{ + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = createPoint(x, y, z); + p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + return p; +} + +geometry_msgs::msg::Pose createPose(const std::array & pose3d) +{ + return createPose(pose3d[0], pose3d[1], pose3d[2], 0.0, 0.0, pose3d[3]); +} + +LaneletSegment createLaneletSegment(int id) +{ + LaneletPrimitive primitive; + primitive.id = id; + primitive.primitive_type = "lane"; + LaneletSegment segment; + segment.preferred_primitive.id = id; + segment.primitives.push_back(primitive); + return segment; +} + +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (errors.empty()) { + return map; + } + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; +} + +LaneletMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) +{ + std::string format_version{}; + std::string map_version{}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + + LaneletMapBin map_bin_msg; + map_bin_msg.header.stamp = now; + map_bin_msg.header.frame_id = "map"; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; + lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); + + return map_bin_msg; +} + +LaneletRoute makeNormalRoute() +{ + const std::array start_pose{5.5, 4., 0., M_PI_2}; + const std::array goal_pose{8.0, 26.3, 0, 0}; + LaneletRoute route; + route.header.frame_id = "map"; + route.start_pose = createPose(start_pose); + route.goal_pose = createPose(goal_pose); + return route; +} + +OccupancyGrid makeCostMapMsg(size_t width, size_t height, double resolution) +{ + nav_msgs::msg::OccupancyGrid costmap_msg; + + // create info + costmap_msg.header.frame_id = "map"; + costmap_msg.info.width = width; + costmap_msg.info.height = height; + costmap_msg.info.resolution = static_cast(resolution); + + // create data + const size_t n_elem = width * height; + for (size_t i = 0; i < n_elem; ++i) { + costmap_msg.data.push_back(0.0); + } + return costmap_msg; +} + +LaneletMapBin make_map_bin_msg( + const std::string & absolute_path, const double center_line_resolution) +{ + const auto map = loadMap(absolute_path); + if (!map) { + return autoware_map_msgs::msg::LaneletMapBin_>{}; + } + + // overwrite centerline + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + + // create map bin msg + const auto map_bin_msg = + convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now()); + return map_bin_msg; +} + +LaneletMapBin makeMapBinMsg() +{ + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; + double center_line_resolution = 5.0; + + return make_map_bin_msg(lanelet2_path, center_line_resolution); +} + +Odometry makeOdometry(const double shift) +{ + Odometry odometry; + const std::array start_pose{0.0, shift, 0.0, 0.0}; + odometry.pose.pose = createPose(start_pose); + odometry.header.frame_id = "map"; + return odometry; +} + +Odometry makeInitialPose(const double shift) +{ + Odometry current_odometry; + const auto yaw = 0.9724497591854532; + const auto shift_x = shift * std::sin(yaw); + const auto shift_y = shift * std::cos(yaw); + const std::array start_pose{ + 3722.16015625 + shift_x, 73723.515625 + shift_y, 0.233112560494183, yaw}; + current_odometry.pose.pose = test_utils::createPose(start_pose); + current_odometry.header.frame_id = "map"; + return current_odometry; +} + +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string && frame_id, std::string && child_frame_id) +{ + TFMessage tf_msg; + geometry_msgs::msg::Quaternion quaternion; + quaternion.x = 0.; + quaternion.y = 0.; + quaternion.z = 0.23311256049418302; + quaternion.w = 0.9724497591854532; + + TransformStamped tf; + tf.header.stamp = target_node->get_clock()->now(); + tf.header.frame_id = frame_id; + tf.child_frame_id = child_frame_id; + tf.transform.translation.x = 3722.16015625; + tf.transform.translation.y = 73723.515625; + tf.transform.translation.z = 0; + tf.transform.rotation = quaternion; + + tf_msg.transforms.emplace_back(std::move(tf)); + return tf_msg; +} + +Scenario makeScenarioMsg(const std::string & scenario) +{ + Scenario scenario_msg; + scenario_msg.current_scenario = scenario; + scenario_msg.activating_scenarios = {scenario}; + return scenario_msg; +} + +RouteSections combineConsecutiveRouteSections( + const RouteSections & route_sections1, const RouteSections & route_sections2) +{ + RouteSections route_sections; + route_sections.reserve(route_sections1.size() + route_sections2.size()); + if (!route_sections1.empty()) { + // remove end route section because it is overlapping with first one in next route_section + route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1); + } + if (!route_sections2.empty()) { + route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end()); + } + return route_sections; +} + +// this is for the test lanelet2_map.osm +// file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 +LaneletRoute makeBehaviorNormalRoute() +{ + LaneletRoute route; + route.header.frame_id = "map"; + route.start_pose = + createPose({3722.16015625, 73723.515625, 0.233112560494183, 0.9724497591854532}); + route.goal_pose = + createPose({3778.362060546875, 73721.2734375, -0.5107480274693206, 0.8597304533609347}); + + std::vector primitive_ids = {9102, 9178, 54, 112}; + for (int id : primitive_ids) { + route.segments.push_back(createLaneletSegment(id)); + } + + std::array uuid_bytes{210, 87, 16, 126, 98, 151, 58, 28, + 252, 221, 230, 92, 122, 170, 46, 6}; + route.uuid.uuid = uuid_bytes; + + route.allow_modification = false; + return route; +} + +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, const int repeat_count) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node); + executor.add_node(target_node); + for (int i = 0; i < repeat_count; i++) { + executor.spin_some(std::chrono::milliseconds(100)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } +} + +void updateNodeOptions( + rclcpp::NodeOptions & node_options, const std::vector & params_files) +{ + std::vector arguments; + arguments.push_back("--ros-args"); + arguments.push_back("--params-file"); + for (const auto & param_file : params_files) { + arguments.push_back(param_file.c_str()); + arguments.push_back("--params-file"); + } + arguments.pop_back(); + + node_options.arguments(std::vector{arguments.begin(), arguments.end()}); +} + +PathWithLaneId loadPathWithLaneIdInYaml() +{ + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto yaml_path = planning_test_utils_dir + "/config/path_with_lane_id_data.yaml"; + YAML::Node yaml_node = YAML::LoadFile(yaml_path); + PathWithLaneId path_msg; + + // Convert YAML data to PathWithLaneId message + // Fill the header + path_msg.header.stamp.sec = yaml_node["header"]["stamp"]["sec"].as(); + path_msg.header.stamp.nanosec = yaml_node["header"]["stamp"]["nanosec"].as(); + path_msg.header.frame_id = yaml_node["header"]["frame_id"].as(); + + // Fill the points + for (const auto & point_node : yaml_node["points"]) { + PathPointWithLaneId point; + // Fill the PathPoint data + point.point.pose.position.x = point_node["point"]["pose"]["position"]["x"].as(); + point.point.pose.position.y = point_node["point"]["pose"]["position"]["y"].as(); + point.point.pose.position.z = point_node["point"]["pose"]["position"]["z"].as(); + point.point.pose.orientation.x = point_node["point"]["pose"]["orientation"]["x"].as(); + point.point.pose.orientation.y = point_node["point"]["pose"]["orientation"]["y"].as(); + point.point.pose.orientation.z = point_node["point"]["pose"]["orientation"]["z"].as(); + point.point.pose.orientation.w = point_node["point"]["pose"]["orientation"]["w"].as(); + point.point.longitudinal_velocity_mps = + point_node["point"]["longitudinal_velocity_mps"].as(); + point.point.lateral_velocity_mps = point_node["point"]["lateral_velocity_mps"].as(); + point.point.heading_rate_rps = point_node["point"]["heading_rate_rps"].as(); + point.point.is_final = point_node["point"]["is_final"].as(); + // Fill the lane_ids + for (const auto & lane_id_node : point_node["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + + path_msg.points.push_back(point); + } + + // Fill the left_bound + for (const auto & point_node : yaml_node["left_bound"]) { + Point point; + // Fill the Point data (left_bound) + point.x = point_node["x"].as(); + point.y = point_node["y"].as(); + point.z = point_node["z"].as(); + + path_msg.left_bound.push_back(point); + } + + // Fill the right_bound + for (const auto & point_node : yaml_node["right_bound"]) { + Point point; + // Fill the Point data + point.x = point_node["x"].as(); + point.y = point_node["y"].as(); + point.z = point_node["z"].as(); + + path_msg.right_bound.push_back(point); + } + return path_msg; +} + +} // namespace test_utils diff --git a/planning/planning_topic_converter/README.md b/planning/planning_topic_converter/README.md index 7be6979e62281..74137cda0197d 100644 --- a/planning/planning_topic_converter/README.md +++ b/planning/planning_topic_converter/README.md @@ -2,7 +2,7 @@ ## Purpose -This package provides tools that convert topic type among types are defined in . +This package provides tools that convert topic type among types are defined in . ## Inner-workings / Algorithms diff --git a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp b/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp index f214f2a4d5e2a..708f624e97898 100644 --- a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp +++ b/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp @@ -18,18 +18,18 @@ #include "planning_topic_converter/converter_base.hpp" #include "rclcpp/rclcpp.hpp" -#include -#include +#include +#include #include namespace planning_topic_converter { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; class PathToTrajectory : public ConverterBase { diff --git a/planning/planning_topic_converter/package.xml b/planning/planning_topic_converter/package.xml index d07d59f3d2c89..3699f3e53900c 100644 --- a/planning/planning_topic_converter/package.xml +++ b/planning/planning_topic_converter/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto - autoware_auto_planning_msgs + autoware_planning_msgs motion_utils rclcpp rclcpp_components diff --git a/planning/planning_validator/CMakeLists.txt b/planning/planning_validator/CMakeLists.txt index 709e00c5a9a25..455dded2d7e32 100644 --- a/planning/planning_validator/CMakeLists.txt +++ b/planning/planning_validator/CMakeLists.txt @@ -56,7 +56,7 @@ if(BUILD_TESTING) ) ament_target_dependencies(test_planning_validator rclcpp - autoware_auto_planning_msgs + autoware_planning_msgs ) target_link_libraries(test_planning_validator planning_validator_component diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md index a260254159947..9d70e7f78a5bb 100644 --- a/planning/planning_validator/README.md +++ b/planning/planning_validator/README.md @@ -31,10 +31,10 @@ The following features are to be implemented. The `planning_validator` takes in the following inputs: -| Name | Type | Description | -| -------------------- | -------------------------------------- | ---------------------------------------------- | -| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | -| `~/input/trajectory` | autoware_auto_planning_msgs/Trajectory | target trajectory to be validated in this node | +| Name | Type | Description | +| -------------------- | --------------------------------- | ---------------------------------------------- | +| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | +| `~/input/trajectory` | autoware_planning_msgs/Trajectory | target trajectory to be validated in this node | ### Outputs @@ -42,7 +42,7 @@ It outputs the following: | Name | Type | Description | | ---------------------------- | ------------------------------------------ | ------------------------------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs/Trajectory | validated trajectory | +| `~/output/trajectory` | autoware_planning_msgs/Trajectory | validated trajectory | | `~/output/validation_status` | planning_validator/PlanningValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid | | `/diagnostics` | diagnostic_msgs/DiagnosticStatus | diagnostics to report errors | diff --git a/planning/planning_validator/include/planning_validator/debug_marker.hpp b/planning/planning_validator/include/planning_validator/debug_marker.hpp index 4c7f6907a97b2..4671a0b0d7297 100644 --- a/planning/planning_validator/include/planning_validator/debug_marker.hpp +++ b/planning/planning_validator/include/planning_validator/debug_marker.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -32,8 +32,7 @@ class PlanningValidatorDebugMarkerPublisher explicit PlanningValidatorDebugMarkerPublisher(rclcpp::Node * node); void pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, - int id = 0); + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0); void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); void pushVirtualWall(const geometry_msgs::msg::Pose & pose); void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 36954c267b2a9..fed6e161c4d90 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include #include @@ -35,8 +35,8 @@ namespace planning_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/planning_validator/utils.hpp index 5831993d748b6..11eaf4f37f400 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/planning_validator/utils.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -25,8 +25,8 @@ namespace planning_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; std::pair getMaxValAndIdx(const std::vector & v); diff --git a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml index fb64d2e0841bd..3f73e0f462cc2 100644 --- a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml +++ b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml @@ -1,6 +1,6 @@ - - + + diff --git a/planning/planning_validator/launch/planning_validator.launch.xml b/planning/planning_validator/launch/planning_validator.launch.xml index 382c2d22b2adf..81573adb4dc24 100644 --- a/planning/planning_validator/launch/planning_validator.launch.xml +++ b/planning/planning_validator/launch/planning_validator.launch.xml @@ -1,6 +1,6 @@ - + diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 9ecc760efd7e3..5a413f27dc7a3 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -15,7 +15,7 @@ autoware_cmake rosidl_default_generators - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager diagnostic_updater geometry_msgs diff --git a/planning/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp index 1a0793957402a..82117d199c36e 100644 --- a/planning/planning_validator/src/debug_marker.cpp +++ b/planning/planning_validator/src/debug_marker.cpp @@ -40,7 +40,7 @@ void PlanningValidatorDebugMarkerPublisher::clearMarkers() } void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) { pushPoseMarker(p.pose, ns, id); } 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 421b60d0ab756..1d1e47ad10407 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 @@ -17,14 +17,14 @@ #include -#include +#include #include namespace planning_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; class InvalidTrajectoryPublisherNode : public rclcpp::Node { 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 482996c4c040d..e406b6c5d7da2 100644 --- a/planning/planning_validator/test/src/test_planning_validator_functions.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_functions.cpp @@ -21,7 +21,7 @@ #include -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using planning_validator::PlanningValidator; TEST(PlanningValidatorTestSuite, checkValidFiniteValueFunction) diff --git a/planning/planning_validator/test/src/test_planning_validator_helper.cpp b/planning/planning_validator/test/src/test_planning_validator_helper.cpp index 19e0d5a7f73ee..de7d8293286d7 100644 --- a/planning/planning_validator/test/src/test_planning_validator_helper.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_helper.cpp @@ -19,8 +19,8 @@ #include -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::createQuaternionFromYaw; Trajectory generateTrajectoryWithConstantAcceleration( diff --git a/planning/planning_validator/test/src/test_planning_validator_helper.hpp b/planning/planning_validator/test/src/test_planning_validator_helper.hpp index d2e6472fb04bc..23b634c3f56ed 100644 --- a/planning/planning_validator/test/src/test_planning_validator_helper.hpp +++ b/planning/planning_validator/test/src/test_planning_validator_helper.hpp @@ -17,11 +17,11 @@ #include -#include -#include +#include +#include #include -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using nav_msgs::msg::Odometry; Trajectory generateTrajectoryWithConstantAcceleration( 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 a7b90be4471cd..2329a3b9bb6c8 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -29,7 +29,7 @@ * This test checks the diagnostics message published from the planning_validator node */ -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 73bf99962ea82..24a20f86ed718 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -17,11 +17,11 @@ #include -#include -#include +#include #include #include #include +#include #include #include @@ -37,13 +37,13 @@ namespace route_handler { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using std_msgs::msg::Header; +using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; using RouteSections = std::vector; @@ -55,10 +55,10 @@ class RouteHandler { public: RouteHandler() = default; - explicit RouteHandler(const HADMapBin & map_msg); + explicit RouteHandler(const LaneletMapBin & map_msg); // non-const methods - void setMap(const HADMapBin & map_msg); + void setMap(const LaneletMapBin & map_msg); void setRoute(const LaneletRoute & route_msg); void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets); void clearRoute(); diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index 097bc62f8bf39..8be6707e933d7 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -22,8 +22,7 @@ ament_lint_auto autoware_lint_common - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_planning_msgs geometry_msgs lanelet2_extension diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 4dd559b9bbc98..d410cb2668175 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -22,9 +22,9 @@ #include #include -#include -#include #include +#include +#include #include #include @@ -43,12 +43,12 @@ namespace { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::Path; using geometry_msgs::msg::Pose; using lanelet::utils::to2D; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; bool exists(const std::vector & primitives, const int64_t & id) { @@ -133,13 +133,13 @@ std::string toString(const geometry_msgs::msg::Pose & pose) namespace route_handler { -RouteHandler::RouteHandler(const HADMapBin & map_msg) +RouteHandler::RouteHandler(const LaneletMapBin & map_msg) { setMap(map_msg); route_ptr_ = nullptr; } -void RouteHandler::setMap(const HADMapBin & map_msg) +void RouteHandler::setMap(const LaneletMapBin & map_msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp index 3c6893da3f9d1..9f9fba717a4aa 100644 --- a/planning/route_handler/test/test_route_handler.hpp +++ b/planning/route_handler/test/test_route_handler.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include @@ -37,7 +37,7 @@ namespace route_handler::test { -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; class TestRouteHandler : public ::testing::Test { diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 5e4b202071e49..77fe29b41d893 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -62,9 +62,9 @@ Module getModuleType(const std::string & module_name) module.type = Module::AVOIDANCE_BY_LC_LEFT; } else if (module_name == "avoidance_by_lane_change_right") { module.type = Module::AVOIDANCE_BY_LC_RIGHT; - } else if (module_name == "avoidance_left") { + } else if (module_name == "static_obstacle_avoidance_left") { module.type = Module::AVOIDANCE_LEFT; - } else if (module_name == "avoidance_right") { + } else if (module_name == "static_obstacle_avoidance_right") { module.type = Module::AVOIDANCE_RIGHT; } else if (module_name == "goal_planner") { module.type = Module::GOAL_PLANNER; @@ -153,7 +153,16 @@ std::vector RTCInterface::validateCooperateCommands( registered_status_.statuses.begin(), registered_status_.statuses.end(), [command](auto & s) { return s.uuid == command.uuid; }); if (itr != registered_status_.statuses.end()) { - response.success = true; + if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) { + response.success = true; + } else { + RCLCPP_WARN_STREAM( + getLogger(), "[validateCooperateCommands] uuid : " + << to_string(command.uuid) + << " state is not WAITING_FOR_EXECUTION or RUNNING. state : " + << itr->state.type << std::endl); + response.success = false; + } } else { RCLCPP_WARN_STREAM( getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid) @@ -175,8 +184,10 @@ void RTCInterface::updateCooperateCommandStatus(const std::vectorcommand_status = command.command; - itr->auto_mode = false; + if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) { + itr->command_status = command.command; + itr->auto_mode = false; + } } } } @@ -219,7 +230,7 @@ void RTCInterface::updateCooperateStatus( status.module = module_; status.safe = safe; status.command_status.type = Command::DEACTIVATE; - status.state.type = State::WAITING_FOR_EXECUTION; + status.state.type = state; status.start_distance = start_distance; status.finish_distance = finish_distance; status.auto_mode = is_auto_mode_enabled_; @@ -291,6 +302,9 @@ bool RTCInterface::isActivated(const UUID & uuid) const [uuid](auto & s) { return s.uuid == uuid; }); if (itr != registered_status_.statuses.end()) { + if (itr->state.type == State::FAILED || itr->state.type == State::SUCCEEDED) { + return false; + } if (itr->auto_mode) { return itr->safe; } else { diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp index 5b395883dd65d..6f0bef882743c 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -22,7 +22,7 @@ #include #include -#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path.hpp" #include #include diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/path_sampler/README.md index 2d7fe52e5dcad..27009719c230c 100644 --- a/planning/sampling_based_planner/path_sampler/README.md +++ b/planning/sampling_based_planner/path_sampler/README.md @@ -19,17 +19,17 @@ Note that the velocity is just taken over from the input path. ### input -| Name | Type | Description | -| ------------------ | -------------------------------------------------- | -------------------------------------------------- | -| `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | -| `~/input/odometry` | nav_msgs/msg/Odometry | Current state of the ego vehicle | -| `~/input/objects` | autoware_auto_perception_msgs/msg/PredictedObjects | objects to avoid | +| Name | Type | Description | +| ------------------ | --------------------------------------------- | -------------------------------------------------- | +| `~/input/path` | autoware_planning_msgs/msg/Path | Reference path and the corresponding drivable area | +| `~/input/odometry` | nav_msgs/msg/Odometry | Current state of the ego vehicle | +| `~/input/objects` | autoware_perception_msgs/msg/PredictedObjects | objects to avoid | ### output -| Name | Type | Description | -| --------------------- | ------------------------------------------ | ----------------------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs/msg/Trajectory | generated trajectory that is feasible to drive and collision-free | +| Name | Type | Description | +| --------------------- | ------------------------------------- | ----------------------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs/msg/Trajectory | generated trajectory that is feasible to drive and collision-free | ## Algorithm @@ -69,9 +69,9 @@ If the reference path is not smooth, the resulting candidates will probably be u Failure to find a valid trajectory current results in a suddenly stopping trajectory. -## Comparison with the `obstacle_avoidance_planner` +## Comparison with the `autoware_path_optimizer` -The `obstacle_avoidance_planner` uses an optimization based approach, +The `autoware_path_optimizer` uses an optimization based approach, finding the optimal solution of a mathematical problem if it exists. When no solution can be found, it is often hard to identify the issue due to the intermediate mathematical representation of the problem. 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 index 41493b74659fc..448400236c9f8 100644 --- 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 @@ -22,7 +22,7 @@ #include "sampler_common/structures.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include +#include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp index 29cfddfbf8632..a9a2728b27c49 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp @@ -20,11 +20,11 @@ #include "path_sampler/type_alias.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp index 8a792eba2f237..90e655c086444 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp @@ -15,11 +15,11 @@ #ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_ #define PATH_SAMPLER__TYPE_ALIAS_HPP_ -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -33,12 +33,12 @@ namespace path_sampler // std_msgs using std_msgs::msg::Header; // planning -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; // perception -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; // navigation using nav_msgs::msg::Odometry; // visualization diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp index 3e5afdbdad696..875b81b9cd49a 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp @@ -24,8 +24,8 @@ #include "path_sampler/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "boost/optional/optional_fwd.hpp" diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp index d848e9b209d28..a036dcfe638c2 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp @@ -24,8 +24,8 @@ #include "path_sampler/type_alias.hpp" #include "sampler_common/structures.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include #include diff --git a/planning/sampling_based_planner/path_sampler/package.xml b/planning/sampling_based_planner/path_sampler/package.xml index 22303a021ff63..a57ff6e6033f6 100644 --- a/planning/sampling_based_planner/path_sampler/package.xml +++ b/planning/sampling_based_planner/path_sampler/package.xml @@ -13,7 +13,8 @@ autoware_cmake - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs bezier_sampler frenet_planner geometry_msgs diff --git a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp index 4ae6382b82383..2d1587c7b45a3 100644 --- a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp +++ b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp index ad516faa717ff..07bd4f32787de 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp @@ -19,8 +19,8 @@ #include "path_sampler/utils/geometry_utils.hpp" #include "tf2/utils.h" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/scenario_selector/README.md b/planning/scenario_selector/README.md index 0aea2c876cf35..373ac164e5057 100644 --- a/planning/scenario_selector/README.md +++ b/planning/scenario_selector/README.md @@ -6,21 +6,21 @@ ### Input topics -| Name | Type | Description | -| -------------------------------- | --------------------------------------- | ----------------------------------------------------- | -| `~input/lane_driving/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory of LaneDriving scenario | -| `~input/parking/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory of Parking scenario | -| `~input/lanelet_map` | autoware_auto_mapping_msgs::HADMapBin | | -| `~input/route` | autoware_planning_msgs::LaneletRoute | route and goal pose | -| `~input/odometry` | nav_msgs::Odometry | for checking whether vehicle is stopped | -| `is_parking_completed` | bool (implemented as rosparam) | whether all split trajectory of Parking are published | +| Name | Type | Description | +| -------------------------------- | ------------------------------------- | ----------------------------------------------------- | +| `~input/lane_driving/trajectory` | autoware_planning_msgs::Trajectory | trajectory of LaneDriving scenario | +| `~input/parking/trajectory` | autoware_planning_msgs::Trajectory | trajectory of Parking scenario | +| `~input/lanelet_map` | autoware_map_msgs::msg::LaneletMapBin | | +| `~input/route` | autoware_planning_msgs::LaneletRoute | route and goal pose | +| `~input/odometry` | nav_msgs::Odometry | for checking whether vehicle is stopped | +| `is_parking_completed` | bool (implemented as rosparam) | whether all split trajectory of Parking are published | ### Output topics -| Name | Type | Description | -| -------------------- | --------------------------------------- | ---------------------------------------------- | -| `~output/scenario` | tier4_planning_msgs::Scenario | current scenario and scenarios to be activated | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | +| Name | Type | Description | +| -------------------- | ---------------------------------- | ---------------------------------------------- | +| `~output/scenario` | tier4_planning_msgs::Scenario | current scenario and scenarios to be activated | +| `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | ### Output TFs diff --git a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp index 3651f6c76b8bc..7162a435d8852 100644 --- a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp +++ b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp @@ -18,9 +18,9 @@ #include #include -#include -#include +#include #include +#include #include #include #include @@ -48,39 +48,37 @@ class ScenarioSelectorNode : public rclcpp::Node public: explicit ScenarioSelectorNode(const rclcpp::NodeOptions & node_options); - void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void onParkingState(const std_msgs::msg::Bool::ConstSharedPtr msg); bool isDataReady(); void onTimer(); - void onLaneDrivingTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - void onParkingTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - void publishTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onLaneDrivingTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onParkingTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void publishTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); void updateCurrentScenario(); std::string selectScenarioByPosition(); - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr getScenarioTrajectory( + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr getScenarioTrajectory( const std::string & scenario); private: rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_lane_driving_trajectory_; - rclcpp::Subscription::SharedPtr - sub_parking_trajectory_; + rclcpp::Subscription::SharedPtr sub_parking_trajectory_; rclcpp::Subscription::SharedPtr sub_parking_state_; - rclcpp::Publisher::SharedPtr pub_trajectory_; + rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr lane_driving_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr parking_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr lane_driving_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr parking_trajectory_; autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route_; nav_msgs::msg::Odometry::ConstSharedPtr current_pose_; geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; diff --git a/planning/scenario_selector/package.xml b/planning/scenario_selector/package.xml index 341076505d5b8..f910c09dbb070 100644 --- a/planning/scenario_selector/package.xml +++ b/planning/scenario_selector/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs + autoware_planning_msgs autoware_planning_test_manager lanelet2_extension nav_msgs diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index 7cc251a03296c..2ba56a911bbeb 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -91,7 +91,7 @@ bool isInParkingLot( } bool isNearTrajectoryEnd( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory, + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory, const geometry_msgs::msg::Pose & current_pose, const double th_dist) { if (!trajectory || trajectory->points.empty()) { @@ -120,8 +120,8 @@ bool isStopped( } // namespace -autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr -ScenarioSelectorNode::getScenarioTrajectory(const std::string & scenario) +autoware_planning_msgs::msg::Trajectory::ConstSharedPtr ScenarioSelectorNode::getScenarioTrajectory( + const std::string & scenario) { if (scenario == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { return lane_driving_trajectory_; @@ -185,8 +185,7 @@ void ScenarioSelectorNode::updateCurrentScenario() } } -void ScenarioSelectorNode::onMap( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +void ScenarioSelectorNode::onMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( @@ -290,7 +289,7 @@ void ScenarioSelectorNode::onTimer() } void ScenarioSelectorNode::onLaneDrivingTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { lane_driving_trajectory_ = msg; @@ -302,7 +301,7 @@ void ScenarioSelectorNode::onLaneDrivingTrajectory( } void ScenarioSelectorNode::onParkingTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { parking_trajectory_ = msg; @@ -314,7 +313,7 @@ void ScenarioSelectorNode::onParkingTrajectory( } void ScenarioSelectorNode::publishTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { const auto now = this->now(); const auto delay_sec = (now - msg->header.stamp).seconds(); @@ -340,16 +339,15 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti is_parking_completed_(false) { // Input - sub_lane_driving_trajectory_ = - this->create_subscription( - "input/lane_driving/trajectory", rclcpp::QoS{1}, - std::bind(&ScenarioSelectorNode::onLaneDrivingTrajectory, this, std::placeholders::_1)); + sub_lane_driving_trajectory_ = this->create_subscription( + "input/lane_driving/trajectory", rclcpp::QoS{1}, + std::bind(&ScenarioSelectorNode::onLaneDrivingTrajectory, this, std::placeholders::_1)); - sub_parking_trajectory_ = this->create_subscription( + sub_parking_trajectory_ = this->create_subscription( "input/parking/trajectory", rclcpp::QoS{1}, std::bind(&ScenarioSelectorNode::onParkingTrajectory, this, std::placeholders::_1)); - sub_lanelet_map_ = this->create_subscription( + sub_lanelet_map_ = this->create_subscription( "input/lanelet_map", rclcpp::QoS{1}.transient_local(), std::bind(&ScenarioSelectorNode::onMap, this, std::placeholders::_1)); sub_route_ = this->create_subscription( @@ -365,7 +363,7 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti // Output pub_scenario_ = this->create_publisher("output/scenario", rclcpp::QoS{1}); - pub_trajectory_ = this->create_publisher( + pub_trajectory_ = this->create_publisher( "output/trajectory", rclcpp::QoS{1}); // Timer Callback diff --git a/planning/surround_obstacle_checker/README.md b/planning/surround_obstacle_checker/README.md index 071701c024685..5cf7c8a419b61 100644 --- a/planning/surround_obstacle_checker/README.md +++ b/planning/surround_obstacle_checker/README.md @@ -78,13 +78,13 @@ As mentioned in stop condition section, it prevents chattering by changing thres ### Input -| Name | Type | Description | -| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ------------------------------------------------- | ------------------------------------------------------------------ | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index a720de68acbcd..e009c77d3ea1b 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include @@ -45,8 +45,8 @@ namespace surround_obstacle_checker { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; using motion_utils::VehicleStopChecker; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 1f99ba8d2fcba..d9b5d4615e148 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -14,9 +14,8 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 + autoware_perception_msgs + autoware_planning_msgs diagnostic_msgs eigen libpcl-all-dev diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 4c7d0589f843b..c19a04bbfeac0 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -14,7 +14,6 @@ #include "surround_obstacle_checker/node.hpp" -#include #include #include #include @@ -52,7 +51,7 @@ namespace surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; -using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::pose2transform; diff --git a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md index 8a72c976621f8..f8a5f29e064f7 100644 --- a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md +++ b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md @@ -78,13 +78,13 @@ Stop condition の項で述べたように、状態によって障害物判定 ### Input -| Name | Type | Description | -| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ------------------------------------------------- | ------------------------------------------------------------------ | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 90c3553312dc4..c33f9a7589812 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-filter.md index fedd3c081cdeb..c38a4c719fcf3 100644 --- a/sensing/pointcloud_preprocessor/docs/vector-map-filter.md +++ b/sensing/pointcloud_preprocessor/docs/vector-map-filter.md @@ -10,10 +10,10 @@ The `vector_map_filter` is a node that removes points on the outside of lane by ### Input -| Name | Type | Description | -| -------------------- | -------------------------------------------- | ---------------- | -| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | -| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| Name | Type | Description | +| -------------------- | --------------------------------------- | ---------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | ### Output 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 4bafac7872e78..bd2f7d98919d5 100644 --- a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md +++ b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md @@ -19,10 +19,10 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, so please ### Input -| Name | Type | Description | -| -------------------- | -------------------------------------------- | ------------------------------------ | -| `~/input` | `sensor_msgs::msg::PointCloud2` | input points | -| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map used for filtering points | +| Name | Type | Description | +| -------------------- | --------------------------------------- | ------------------------------------ | +| `~/input` | `sensor_msgs::msg::PointCloud2` | input points | +| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map used for filtering points | ### Output diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index f46d6595b2a76..4e183a9816c2e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -68,7 +68,7 @@ #include #include -#include +#include #include #include #include @@ -127,7 +127,7 @@ class PointCloudConcatenationComponent : public rclcpp::Node /** \brief A vector of subscriber. */ std::vector::SharedPtr> filters_; - rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_twist_; rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index ed0d753a68ecc..42078922d39b9 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -59,7 +59,7 @@ class Lanelet2MapFilterComponent : public rclcpp::Node std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr pointcloud_sub_; rclcpp::Publisher::SharedPtr filtered_pointcloud_pub_; @@ -71,7 +71,7 @@ class Lanelet2MapFilterComponent : public rclcpp::Node void pointcloudCallback(const PointCloud2ConstPtr msg); - void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); bool transformPointCloud( const std::string & in_target_frame, const PointCloud2ConstPtr & in_cloud_ptr, 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 74209c1a22e4c..894768d385438 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 @@ -36,10 +36,10 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; - rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr map_sub_; lanelet::ConstPolygons3d polygon_lanelets_; - void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); // parameter std::string polygon_type_; diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 47fa39e7fcda2..619bf3180591d 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -12,6 +12,8 @@ Kenzo Lobos-Tsunekawa Yihsiang Fang Yoshi Ri + David Wong + Melike Tanrikulu Apache License 2.0 diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp index 502c85d1746e8..8f7cb2abec6a4 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp @@ -51,7 +51,7 @@ Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions // Set subscriber { - map_sub_ = this->create_subscription( + map_sub_ = this->create_subscription( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&Lanelet2MapFilterComponent::mapCallback, this, _1)); pointcloud_sub_ = this->create_subscription( @@ -259,7 +259,7 @@ void Lanelet2MapFilterComponent::pointcloudCallback(const PointCloud2ConstPtr cl } void Lanelet2MapFilterComponent::mapCallback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr map_msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr map_msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_); 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 5b8755714b375..8fcf15326829f 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 @@ -71,7 +71,7 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent( using std::placeholders::_1; // Set subscriber - map_sub_ = this->create_subscription( + map_sub_ = this->create_subscription( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1)); } @@ -107,7 +107,7 @@ void VectorMapInsideAreaFilterComponent::filter( } void VectorMapInsideAreaFilterComponent::mapCallback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr map_msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr map_msg) { tf_input_frame_ = map_msg->header.frame_id; diff --git a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml b/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml index 028ca5de6a220..f980bcebf1b6b 100644 --- a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml +++ b/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/sensing/vehicle_velocity_converter/README.md b/sensing/vehicle_velocity_converter/README.md index 92881f9321f28..3c7292f3fcdc4 100644 --- a/sensing/vehicle_velocity_converter/README.md +++ b/sensing/vehicle_velocity_converter/README.md @@ -2,15 +2,15 @@ ## Purpose -This package converts autoware_auto_vehicle_msgs::msg::VehicleReport message to geometry_msgs::msg::TwistWithCovarianceStamped for gyro odometer node. +This package converts autoware_vehicle_msgs::msg::VehicleReport message to geometry_msgs::msg::TwistWithCovarianceStamped for gyro odometer node. ## Inputs / Outputs ### Input -| Name | Type | Description | -| ----------------- | ------------------------------------------------ | ---------------- | -| `velocity_status` | `autoware_auto_vehicle_msgs::msg::VehicleReport` | vehicle velocity | +| Name | Type | Description | +| ----------------- | ------------------------------------------- | ---------------- | +| `velocity_status` | `autoware_vehicle_msgs::msg::VehicleReport` | vehicle velocity | ### Output 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 660ad330f07f3..4a1a66b842892 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 @@ -17,7 +17,7 @@ #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -32,10 +32,9 @@ class VehicleVelocityConverter : public rclcpp::Node ~VehicleVelocityConverter() = default; private: - void callbackVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg); + void callbackVelocityReport(const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg); - rclcpp::Subscription::SharedPtr - vehicle_report_sub_; + rclcpp::Subscription::SharedPtr vehicle_report_sub_; rclcpp::Publisher::SharedPtr twist_with_covariance_pub_; diff --git a/sensing/vehicle_velocity_converter/package.xml b/sensing/vehicle_velocity_converter/package.xml index c44c55bcd40eb..39780deaccc28 100644 --- a/sensing/vehicle_velocity_converter/package.xml +++ b/sensing/vehicle_velocity_converter/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs geometry_msgs rclcpp ament_lint_auto diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index 0f4a22bbc9730..360ec4cae58d5 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -22,7 +22,7 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_co frame_id_ = declare_parameter("frame_id"); speed_scale_factor_ = declare_parameter("speed_scale_factor"); - vehicle_report_sub_ = create_subscription( + vehicle_report_sub_ = create_subscription( "velocity_status", rclcpp::QoS{100}, std::bind(&VehicleVelocityConverter::callbackVelocityReport, this, std::placeholders::_1)); @@ -31,7 +31,7 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_co } void VehicleVelocityConverter::callbackVelocityReport( - const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) + const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { if (msg->header.frame_id != frame_id_) { RCLCPP_WARN(get_logger(), "frame_id is not base_link."); diff --git a/simulator/dummy_perception_publisher/CMakeLists.txt b/simulator/dummy_perception_publisher/CMakeLists.txt index 63d07faf0634c..a27667edf536b 100644 --- a/simulator/dummy_perception_publisher/CMakeLists.txt +++ b/simulator/dummy_perception_publisher/CMakeLists.txt @@ -7,14 +7,14 @@ autoware_package() rosidl_generate_interfaces(${PROJECT_NAME} "msg/InitialState.msg" "msg/Object.msg" - DEPENDENCIES autoware_auto_perception_msgs tier4_perception_msgs geometry_msgs std_msgs unique_identifier_msgs + DEPENDENCIES autoware_perception_msgs tier4_perception_msgs geometry_msgs std_msgs unique_identifier_msgs ) # See ndt_omp package for documentation on why PCL is special find_package(PCL REQUIRED COMPONENTS common filters) set(${PROJECT_NAME}_DEPENDENCIES - autoware_auto_perception_msgs + autoware_perception_msgs tier4_perception_msgs pcl_conversions rclcpp diff --git a/simulator/dummy_perception_publisher/README.md b/simulator/dummy_perception_publisher/README.md index 48036948ece65..5d969dd292832 100644 --- a/simulator/dummy_perception_publisher/README.md +++ b/simulator/dummy_perception_publisher/README.md @@ -21,7 +21,7 @@ This node publishes the result of the dummy detection with the type of perceptio | ----------------------------------- | -------------------------------------------------------- | ----------------------- | | `output/dynamic_object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | dummy detection objects | | `output/points_raw` | `sensor_msgs::msg::PointCloud2` | point cloud of objects | -| `output/debug/ground_truth_objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | ground truth objects | +| `output/debug/ground_truth_objects` | `autoware_perception_msgs::msg::TrackedObjects` | ground truth objects | ## Parameters diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index 770663e84dc0c..c3f7976d3efa1 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -19,8 +19,8 @@ #include -#include -#include +#include +#include #include #include @@ -59,7 +59,7 @@ struct ObjectInfo geometry_msgs::msg::PoseWithCovariance pose_covariance_; // convert to TrackedObject // (todo) currently need object input to get id and header information, but it should be removed - autoware_auto_perception_msgs::msg::TrackedObject toTrackedObject( + autoware_perception_msgs::msg::TrackedObject toTrackedObject( const dummy_perception_publisher::msg::Object & object) const; }; @@ -114,7 +114,7 @@ class DummyPerceptionPublisherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr detected_object_with_feature_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr ground_truth_objects_pub_; rclcpp::Subscription::SharedPtr object_sub_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/simulator/dummy_perception_publisher/msg/Object.msg b/simulator/dummy_perception_publisher/msg/Object.msg index 30279299dbfb1..11ac1b3d39daa 100644 --- a/simulator/dummy_perception_publisher/msg/Object.msg +++ b/simulator/dummy_perception_publisher/msg/Object.msg @@ -1,8 +1,8 @@ std_msgs/Header header unique_identifier_msgs/UUID id dummy_perception_publisher/InitialState initial_state -autoware_auto_perception_msgs/ObjectClassification classification -autoware_auto_perception_msgs/Shape shape +autoware_perception_msgs/ObjectClassification classification +autoware_perception_msgs/Shape shape float32 max_velocity float32 min_velocity diff --git a/simulator/dummy_perception_publisher/package.xml b/simulator/dummy_perception_publisher/package.xml index 0632bd90b2c22..4704024214d9e 100644 --- a/simulator/dummy_perception_publisher/package.xml +++ b/simulator/dummy_perception_publisher/package.xml @@ -18,7 +18,7 @@ ament_lint_auto autoware_lint_common - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs libpcl-all-dev pcl_conversions diff --git a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp index 9b3f01fa4b267..2d1ea626fb1ac 100644 --- a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp +++ b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp @@ -14,7 +14,7 @@ #include -#include +#include #include #include @@ -24,9 +24,8 @@ class EmptyObjectsPublisher : public rclcpp::Node public: EmptyObjectsPublisher() : Node("empty_objects_publisher") { - empty_objects_pub_ = - this->create_publisher( - "~/output/objects", 1); + empty_objects_pub_ = this->create_publisher( + "~/output/objects", 1); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -34,13 +33,12 @@ class EmptyObjectsPublisher : public rclcpp::Node } private: - rclcpp::Publisher::SharedPtr - empty_objects_pub_; + rclcpp::Publisher::SharedPtr empty_objects_pub_; rclcpp::TimerBase::SharedPtr timer_; void timerCallback() { - autoware_auto_perception_msgs::msg::PredictedObjects empty_objects; + autoware_perception_msgs::msg::PredictedObjects empty_objects; empty_objects.header.frame_id = "map"; empty_objects.header.stamp = this->now(); empty_objects_pub_->publish(empty_objects); diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 3ac663d2c8647..9c58961978161 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -33,8 +33,8 @@ #include #include -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; ObjectInfo::ObjectInfo( const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time) @@ -169,7 +169,7 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() // optional ground truth publisher if (publish_ground_truth_objects_) { ground_truth_objects_pub_ = - this->create_publisher( + this->create_publisher( "~/output/debug/ground_truth_objects", qos); } @@ -182,7 +182,7 @@ void DummyPerceptionPublisherNode::timerCallback() { // output msgs tier4_perception_msgs::msg::DetectedObjectsWithFeature output_dynamic_object_msg; - autoware_auto_perception_msgs::msg::TrackedObjects output_ground_truth_objects_msg; + autoware_perception_msgs::msg::TrackedObjects output_ground_truth_objects_msg; geometry_msgs::msg::PoseStamped output_moved_object_pose; sensor_msgs::msg::PointCloud2 output_pointcloud_msg; std_msgs::msg::Header header; @@ -282,7 +282,7 @@ void DummyPerceptionPublisherNode::timerCallback() feature_object.object.kinematics.twist_with_covariance = object.initial_state.twist_covariance; feature_object.object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; feature_object.object.kinematics.has_twist = false; tf2::toMsg( tf_base_link2noised_moved_object, diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 1f8762b722a29..4902832f836a9 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -18,23 +18,23 @@ The purpose of this simulator is for the integration test of planning and contro ### input - input/initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose -- input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle -- input/manual_ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : manual target command to drive a vehicle (used when control_mode_request = Manual) -- input/gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command. -- input/manual_gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command (used when control_mode_request = Manual) -- input/turn_indicators_command [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand`] : target turn indicator command -- input/hazard_lights_command [`autoware_auto_vehicle_msgs/msg/HazardLightsCommand`] : target hazard lights command +- input/ackermann_control_command [`autoware_control_msgs/msg/Control`] : target command to drive a vehicle +- input/manual_ackermann_control_command [`autoware_control_msgs/msg/Control`] : manual target command to drive a vehicle (used when control_mode_request = Manual) +- input/gear_command [`autoware_vehicle_msgs/msg/GearCommand`] : target gear command. +- input/manual_gear_command [`autoware_vehicle_msgs/msg/GearCommand`] : target gear command (used when control_mode_request = Manual) +- input/turn_indicators_command [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`] : target turn indicator command +- input/hazard_lights_command [`autoware_vehicle_msgs/msg/HazardLightsCommand`] : target hazard lights command - input/control_mode_request [`tier4_vehicle_msgs::srv::ControlModeRequest`] : mode change for Auto/Manual driving ### output - /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link) - /output/odometry [`nav_msgs/msg/Odometry`] : simulated vehicle pose and twist -- /output/steering [`autoware_auto_vehicle_msgs/msg/SteeringReport`] : simulated steering angle -- /output/control_mode_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : current control mode (Auto/Manual) -- /output/gear_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated gear -- /output/turn_indicators_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated turn indicator status -- /output/hazard_lights_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated hazard lights status +- /output/steering [`autoware_vehicle_msgs/msg/SteeringReport`] : simulated steering angle +- /output/control_mode_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : current control mode (Auto/Manual) +- /output/gear_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : simulated gear +- /output/turn_indicators_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : simulated turn indicator status +- /output/hazard_lights_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : simulated hazard lights status ## Inner-workings / Algorithms diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index e81d4fef00abb..18a3a3bae501a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -20,23 +20,20 @@ #include "simple_planning_simulator/visibility_control.hpp" #include "tier4_api_utils/tier4_api_utils.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" -#include "autoware_auto_geometry_msgs/msg/complex32.hpp" -#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/control_mode_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/engage.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" -#include "autoware_auto_vehicle_msgs/srv/control_mode_command.hpp" +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/control_mode_report.hpp" +#include "autoware_vehicle_msgs/msg/engage.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_vehicle_msgs/msg/hazard_lights_command.hpp" +#include "autoware_vehicle_msgs/msg/hazard_lights_report.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_vehicle_msgs/msg/turn_indicators_command.hpp" +#include "autoware_vehicle_msgs/msg/turn_indicators_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/srv/control_mode_command.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -62,22 +59,20 @@ namespace simulation namespace simple_planning_simulator { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_geometry_msgs::msg::Complex32; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::ControlModeReport; -using autoware_auto_vehicle_msgs::msg::Engage; -using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::GearReport; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::HazardLightsReport; -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; -using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; -using autoware_auto_vehicle_msgs::msg::VelocityReport; -using autoware_auto_vehicle_msgs::srv::ControlModeCommand; +using autoware_control_msgs::msg::Control; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::ControlModeReport; +using autoware_vehicle_msgs::msg::Engage; +using autoware_vehicle_msgs::msg::GearCommand; +using autoware_vehicle_msgs::msg::GearReport; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::HazardLightsReport; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsReport; +using autoware_vehicle_msgs::msg::VelocityReport; +using autoware_vehicle_msgs::srv::ControlModeCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -143,10 +138,9 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_manual_gear_cmd_; rclcpp::Subscription::SharedPtr sub_turn_indicators_cmd_; rclcpp::Subscription::SharedPtr sub_hazard_lights_cmd_; - rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; - rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; - rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_init_pose_; rclcpp::Subscription::SharedPtr sub_init_twist_; rclcpp::Subscription::SharedPtr sub_trajectory_; @@ -176,8 +170,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node VelocityReport current_velocity_{}; Odometry current_odometry_{}; SteeringReport current_steer_{}; - AckermannControlCommand current_ackermann_cmd_{}; - AckermannControlCommand current_manual_ackermann_cmd_{}; + Control current_ackermann_cmd_{}; + Control current_manual_ackermann_cmd_{}; GearCommand current_gear_cmd_{}; GearCommand current_manual_gear_cmd_{}; TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_{}; @@ -215,15 +209,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer - /** - * @brief set current_vehicle_cmd_ptr_ with received message - */ - void on_vehicle_cmd(const VehicleControlCommand::ConstSharedPtr msg); - /** * @brief set input steering, velocity, and acceleration of the vehicle model */ - void set_input(const AckermannControlCommand & cmd, const double acc_by_slope); + void set_input(const Control & cmd, const double acc_by_slope); /** * @brief set current_vehicle_state_ with received message @@ -238,7 +227,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /** * @brief subscribe lanelet map */ - void on_map(const HADMapBin::ConstSharedPtr msg); + void on_map(const LaneletMapBin::ConstSharedPtr msg); /** * @brief set initial pose for simulation with received message diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 35b95bf4b1ae5..1ecb74be41780 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -152,7 +152,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_vehicle_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index d7ca032aa6c48..b83a831341ac1 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -21,8 +21,6 @@ #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include - #include #include #include @@ -202,7 +200,7 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp index 55b5ddb8fcec5..c73cc54f4ea99 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -107,7 +107,7 @@ class SimModelIdealSteerAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_vehicle_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index 981fb0f416d72..1274a1ec28a07 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -17,7 +17,7 @@ #include -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" /** * @class SimModelInterface @@ -31,8 +31,8 @@ class SimModelInterface Eigen::VectorXd state_; //!< @brief vehicle state vector Eigen::VectorXd input_; //!< @brief vehicle input vector - //!< @brief gear command defined in autoware_auto_msgs/GearCommand - uint8_t gear_ = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE; + //!< @brief gear command defined in autoware_vehicle_msgs/GearCommand + uint8_t gear_ = autoware_vehicle_msgs::msg::GearCommand::DRIVE; public: /** @@ -73,7 +73,7 @@ class SimModelInterface /** * @brief set gear - * @param [in] gear gear command defined in autoware_auto_msgs/GearCommand + * @param [in] gear gear command defined in autoware_vehicle_msgs/GearCommand */ void setGear(const uint8_t gear); diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 0070646a20d49..a8eb88f55df19 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -14,7 +14,6 @@ import launch from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -78,22 +77,7 @@ def launch_setup(context, *args, **kwargs): ], ) - map_to_odom_tf_publisher = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_map_to_odom_tf_publisher", - output="screen", - arguments=[ - "--frame-id", - "map", - "--child-frame-id", - "odom", - ], - ) - - group = GroupAction([simple_planning_simulator_node, map_to_odom_tf_publisher]) - - return [group] + return [simple_planning_simulator_node] def generate_launch_description(): diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 7618ef4204335..5f04fba4fdb8e 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -15,13 +15,10 @@ ament_cmake_auto autoware_cmake - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_map_msgs + autoware_planning_msgs + autoware_vehicle_msgs geometry_msgs lanelet2_core lanelet2_extension @@ -39,6 +36,7 @@ tier4_external_api_msgs tier4_vehicle_msgs vehicle_info_util + autoware_cmake launch_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 7fba0431706f0..3b65218aaf03d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -14,7 +14,6 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" -#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" @@ -44,10 +43,10 @@ using namespace std::literals::chrono_literals; namespace { -autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( +autoware_vehicle_msgs::msg::VelocityReport to_velocity_report( const std::shared_ptr vehicle_model_ptr) { - autoware_auto_vehicle_msgs::msg::VelocityReport velocity; + autoware_vehicle_msgs::msg::VelocityReport velocity; velocity.longitudinal_velocity = static_cast(vehicle_model_ptr->getVx()); velocity.lateral_velocity = 0.0F; velocity.heading_rate = static_cast(vehicle_model_ptr->getWz()); @@ -68,10 +67,10 @@ nav_msgs::msg::Odometry to_odometry( return odometry; } -autoware_auto_vehicle_msgs::msg::SteeringReport to_steering_report( +autoware_vehicle_msgs::msg::SteeringReport to_steering_report( const std::shared_ptr vehicle_model_ptr) { - autoware_auto_vehicle_msgs::msg::SteeringReport steer; + autoware_vehicle_msgs::msg::SteeringReport steer; steer.steering_tire_angle = static_cast(vehicle_model_ptr->getSteer()); return steer; } @@ -109,21 +108,19 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt using std::placeholders::_1; using std::placeholders::_2; - sub_map_ = create_subscription( + sub_map_ = create_subscription( "input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&SimplePlanningSimulator::on_map, this, _1)); sub_init_pose_ = create_subscription( "input/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_init_twist_ = create_subscription( "input/initialtwist", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialtwist, this, _1)); - sub_ackermann_cmd_ = create_subscription( + sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::ConstSharedPtr msg) { current_ackermann_cmd_ = *msg; }); - sub_manual_ackermann_cmd_ = create_subscription( + [this](const Control::ConstSharedPtr msg) { current_ackermann_cmd_ = *msg; }); + sub_manual_ackermann_cmd_ = create_subscription( "input/manual_ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::ConstSharedPtr msg) { - current_manual_ackermann_cmd_ = *msg; - }); + [this](const Control::ConstSharedPtr msg) { current_manual_ackermann_cmd_ = *msg; }); sub_gear_cmd_ = create_subscription( "input/gear_command", QoS{1}, [this](const GearCommand::ConstSharedPtr msg) { current_gear_cmd_ = *msg; }); @@ -420,7 +417,7 @@ void SimplePlanningSimulator::on_timer() publish_tf(current_odometry_); } -void SimplePlanningSimulator::on_map(const HADMapBin::ConstSharedPtr msg) +void SimplePlanningSimulator::on_map(const LaneletMapBin::ConstSharedPtr msg) { auto lanelet_map_ptr = std::make_shared(); @@ -470,14 +467,13 @@ void SimplePlanningSimulator::on_set_pose( response->status = tier4_api_utils::response_success(); } -void SimplePlanningSimulator::set_input( - const AckermannControlCommand & cmd, const double acc_by_slope) +void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by_slope) { const auto steer = cmd.lateral.steering_tire_angle; - const auto vel = cmd.longitudinal.speed; + const auto vel = cmd.longitudinal.velocity; const auto accel = cmd.longitudinal.acceleration; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; Eigen::VectorXd input(vehicle_model_ptr_->getDimU()); const auto gear = vehicle_model_ptr_->getGear(); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 10e6a97d703cd..26b252805db47 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -14,7 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" #include @@ -160,7 +160,7 @@ void SimModelDelaySteerAccGeared::updateStateWithGear( state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); }; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index 72273f0b21ec2..fe847cba946a1 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -14,7 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" #include @@ -134,7 +134,7 @@ Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel( void SimModelDelaySteerMapAccGeared::updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index b2bfe56209938..c2297f1fd1d73 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -14,7 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" #include @@ -92,7 +92,7 @@ void SimModelIdealSteerAccGeared::updateStateWithGear( state(IDX::YAW) = prev_state(IDX::YAW); }; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index cedfec395110e..dc49bf17a11fc 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -24,8 +24,8 @@ #include -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_control_msgs::msg::Control; +using autoware_vehicle_msgs::msg::GearCommand; using geometry_msgs::msg::PoseWithCovarianceStamped; using nav_msgs::msg::Odometry; @@ -51,14 +51,14 @@ class PubSubNode : public rclcpp::Node "output/odometry", rclcpp::QoS{1}, [this](const Odometry::ConstSharedPtr msg) { current_odom_ = msg; }); pub_ackermann_command_ = - create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); + create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); pub_initialpose_ = create_publisher("input/initialpose", rclcpp::QoS{1}); pub_gear_cmd_ = create_publisher("input/gear_command", rclcpp::QoS{1}); } rclcpp::Subscription::SharedPtr current_odom_sub_; - rclcpp::Publisher::SharedPtr pub_ackermann_command_; + rclcpp::Publisher::SharedPtr pub_ackermann_command_; rclcpp::Publisher::SharedPtr pub_gear_cmd_; rclcpp::Publisher::SharedPtr pub_initialpose_; @@ -66,7 +66,7 @@ class PubSubNode : public rclcpp::Node }; /** - * @brief Generate an AckermannControlCommand message + * @brief Generate an Control message * @param [in] t timestamp * @param [in] steer [rad] steering * @param [in] steer_rate [rad/s] steering rotation rate @@ -74,17 +74,17 @@ class PubSubNode : public rclcpp::Node * @param [in] acc [m/s²] acceleration * @param [in] jerk [m/s3] jerk */ -AckermannControlCommand cmdGen( +Control cmdGen( const builtin_interfaces::msg::Time & t, double steer, double steer_rate, double vel, double acc, double jerk) { - AckermannControlCommand cmd; + Control cmd; cmd.stamp = t; cmd.lateral.stamp = t; cmd.lateral.steering_tire_angle = steer; cmd.lateral.steering_tire_rotation_rate = steer_rate; cmd.longitudinal.stamp = t; - cmd.longitudinal.speed = vel; + cmd.longitudinal.velocity = vel; cmd.longitudinal.acceleration = acc; cmd.longitudinal.jerk = jerk; return cmd; @@ -125,8 +125,7 @@ void sendGear( * @param [in] pub_sub_node pointer to the node used for communication */ void sendCommand( - const AckermannControlCommand & cmd, rclcpp::Node::SharedPtr sim_node, - std::shared_ptr pub_sub_node) + const Control & cmd, rclcpp::Node::SharedPtr sim_node, std::shared_ptr pub_sub_node) { for (int i = 0; i < 150; ++i) { pub_sub_node->pub_ackermann_command_->publish(cmd); diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt deleted file mode 100644 index a8b72b8da688c..0000000000000 --- a/system/autoware_auto_msgs_adapter/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_msgs_adapter) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -set(NODE_NAME ${PROJECT_NAME}_node) -set(EXEC_NAME ${PROJECT_NAME}_exe) -set(TEST_NAME test_${PROJECT_NAME}) - -ament_auto_add_library(${NODE_NAME} - src/include/adapter_base.hpp - src/include/adapter_base_interface.hpp - src/include/adapter_control.hpp - src/include/adapter_map.hpp - src/include/adapter_perception.hpp - src/include/adapter_planning.hpp - src/include/autoware_auto_msgs_adapter_core.hpp - src/autoware_auto_msgs_adapter_core.cpp) - -rclcpp_components_register_node(${NODE_NAME} - PLUGIN "autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode" - EXECUTABLE ${EXEC_NAME}) - -if(BUILD_TESTING) - file(GLOB_RECURSE TEST_SOURCES test/*.cpp) - ament_add_ros_isolated_gtest(${TEST_NAME} ${TEST_SOURCES}) - target_include_directories(${TEST_NAME} PRIVATE src/include) - target_link_libraries(${TEST_NAME} ${NODE_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE - launch - config) diff --git a/system/autoware_auto_msgs_adapter/README.md b/system/autoware_auto_msgs_adapter/README.md deleted file mode 100644 index 67fabaaa46c9a..0000000000000 --- a/system/autoware_auto_msgs_adapter/README.md +++ /dev/null @@ -1,102 +0,0 @@ -# autoware_auto_msgs_adapter - -This package is used to convert `autoware_msgs` to `autoware_auto_msgs`. - -## Purpose - -As we transition from `autoware_auto_msgs` to `autoware_msgs`, we wanted to provide flexibility and compatibility for -users who are still using `autoware_auto_msgs`. - -This adapter package allows users to easily convert messages between the two formats. - -## Capabilities - -The `autoware_auto_msgs_adapter` package provides the following capabilities: - -- Conversion of supported `autoware_msgs` messages to `autoware_auto_msgs` messages. -- Can be extended to support conversion for any message type pairs. -- Each instance is designed to convert from a single source message type to a single target message type. -- Multiple instances can be launched to convert multiple message types. -- Can be launched as a standalone node or as a component. - -## Usage - -Customize the adapter configuration by replicating and editing the `autoware_auto_msgs_adapter_control.param.yaml` file located -in the `autoware_auto_msgs_adapter/config` directory. Example configuration: - -```yaml -/**: - ros__parameters: - msg_type_target: "autoware_auto_control_msgs/msg/AckermannControlCommand" - topic_name_source: "/control/command/control_cmd" - topic_name_target: "/control/command/control_cmd_auto" -``` - -Set the `msg_type_target` parameter to the desired target message type from `autoware_auto_msgs`. - -Make sure that the `msg_type_target` has the correspondence in either: - -- [schema/autoware_auto_msgs_adapter.schema.json](schema/autoware_auto_msgs_adapter.schema.json) -- OR [src/autoware_auto_msgs_adapter_core.cpp](src/autoware_auto_msgs_adapter_core.cpp) `AutowareAutoMsgsAdapterNode::create_adapter_map()` method. - -(If this package is maintained correctly, they should match each other.) - -Launch the adapter node by any of the following methods: - -### `ros2 launch` - -```bash -ros2 launch autoware_auto_msgs_adapter autoware_auto_msgs_adapter.launch.xml param_path:='full_path_to_param_file' -``` - -Make sure to set the `param_path` argument to the full path of the parameter file. - -Alternatively, - -- You can replicate and edit the launch file to suit to your needs. -- You can make use of the existing launch file in another launch file by providing the parameter file path as an - argument. - -### `ros2 run` - -```bash -ros2 run autoware_auto_msgs_adapter autoware_auto_msgs_adapter_exe --ros-args --params-file 'full_path_to_param_file' -``` - -Make sure to set the `param_path` argument to the full path of the parameter file. - -## Contributing - -### Current implementation details - -The entry point for the adapter executable is created with `RCLCPP_COMPONENTS_REGISTER_NODE` the [autoware_auto_msgs_adapter_core.cpp](src/Fautoware_auto_msgs_adapter_core.cpp). - -This allows it to be launched as a component or as a standalone node. - -In the `AutowareAutoMsgsAdapterNode` constructor, the adapter is selected by the type string provided in the -configuration file. The adapter is then initialized with the topic names provided. - -The constructors of the adapters are responsible for creating the publisher and subscriber (which makes use of the conversion method). - -### Adding a new message pair - -To add a new message pair, - -- Replicate and edit: - - [adapter_control.hpp](include/autoware_auto_msgs_adapter/adapter_control.hpp). - - Add the new header file to the [CMakeLists.txt](CMakeLists.txt). -- Add a new entry to the returned map instance in the `AutowareAutoMsgsAdapterNode::create_adapter_map()` method of the adapter node: - - [autoware_auto_msgs_adapter_core.cpp](src/autoware_auto_msgs_adapter_core.cpp) -- Add a new entry to the [schema/autoware_auto_msgs_adapter.schema.json](schema/autoware_auto_msgs_adapter.schema.json) file in the `definitions:autoware_auto_msgs_adapter:properties:msg_type_target:enum` section. - - Learn more about JSON schema usage in [here](https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/parameters/#json-schema). -- Create a new config file by replicating and editing: - - [autoware_auto_msgs_adapter_control.param.yaml](config/autoware_auto_msgs_adapter_control.param.yaml) -- Add a new test file by replicating and editing: - - [test_msg_ackermann_control_command.cpp](test/test_msg_ackermann_control_command.cpp) - - No need to edit the `CMakeLists.txt` file as it will automatically detect the new test file. - -Also make sure to test the new adapter with: - -```bash -colcon test --event-handlers console_cohesion+ --packages-select autoware_auto_msgs_adapter -``` diff --git a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml deleted file mode 100644 index 4c6d5f101f380..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_control_msgs/msg/AckermannControlCommand" - topic_name_source: "/control/command/control_cmd" - topic_name_target: "/control/command/control_cmd_auto" diff --git a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml deleted file mode 100644 index dcaa49e089b44..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_mapping_msgs/msg/HADMapBin" - topic_name_source: "/map/vector_map" - topic_name_target: "/map/vector_map_auto" diff --git a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml deleted file mode 100644 index d8b6e31543fad..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_perception_msgs/msg/PredictedObjects" - topic_name_source: "/perception/object_recognition/objects" - topic_name_target: "/perception/object_recognition/objects_auto" diff --git a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml deleted file mode 100644 index b4a8712d4c505..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_planning_msgs/msg/Trajectory" - topic_name_source: "/planning/scenario_planning/trajectory" - topic_name_target: "/planning/scenario_planning/trajectory_auto" diff --git a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml deleted file mode 100755 index 58f2fb4e06238..0000000000000 --- a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml deleted file mode 100644 index 2941820550ba0..0000000000000 --- a/system/autoware_auto_msgs_adapter/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - autoware_auto_msgs_adapter - 1.0.0 - Converts an autoware_msgs message to autoware_auto_msgs version and publishes it. - - M. Fatih Cırıt - Takagi, Isamu - - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - rosidl_default_generators - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_control_msgs - autoware_map_msgs - autoware_perception_msgs - autoware_planning_msgs - rclcpp - rclcpp_components - - - ament_cmake - - diff --git a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json deleted file mode 100644 index a5d6c93029d09..0000000000000 --- a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json +++ /dev/null @@ -1,46 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for autoware_auto_msg_adapter", - "type": "object", - "definitions": { - "autoware_auto_msgs_adapter": { - "type": "object", - "properties": { - "msg_type_target": { - "type": "string", - "description": "Target message type", - "enum": [ - "autoware_auto_control_msgs/msg/AckermannControlCommand", - "autoware_auto_mapping_msgs/msg/HADMapBin", - "autoware_auto_perception_msgs/msg/PredictedObjects", - "autoware_auto_planning_msgs/msg/Trajectory" - ], - "default": "autoware_auto_control_msgs/msg/AckermannControlCommand" - }, - "topic_name_source": { - "type": "string", - "description": "Topic name of the message to be converted.", - "default": "/control/command/control_cmd" - }, - "topic_name_target": { - "type": "string", - "description": "Target topic name which the message will be converted into.", - "default": "/control/command/control_cmd_auto" - } - }, - "required": ["msg_type_target", "topic_name_source", "topic_name_target"] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/autoware_auto_msgs_adapter" - } - }, - "required": ["ros__parameters"] - } - }, - "required": ["/**"] -} diff --git a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp deleted file mode 100644 index 6a23970246866..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "include/autoware_auto_msgs_adapter_core.hpp" - -#include "include/adapter_control.hpp" - -#include - -#include - -namespace autoware_auto_msgs_adapter -{ - -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_control_msgs::msg::Control; - -using MapStringAdapter = AutowareAutoMsgsAdapterNode::MapStringAdapter; - -AutowareAutoMsgsAdapterNode::AutowareAutoMsgsAdapterNode(const rclcpp::NodeOptions & node_options) -: rclcpp::Node("autoware_auto_msgs_adapter", node_options) -{ - const std::string msg_type_target = declare_parameter("msg_type_target"); - const std::string topic_name_source = declare_parameter("topic_name_source"); - const std::string topic_name_target = declare_parameter("topic_name_target"); - - // Map of available adapters - auto map_adapter = create_adapter_map(topic_name_source, topic_name_target); - - print_adapter_options(map_adapter); - - // Initialize the adapter with the selected option - if (!initialize_adapter(map_adapter, msg_type_target)) { - RCLCPP_ERROR( - get_logger(), "Unknown msg type: %s. Please refer to previous log for available options.", - msg_type_target.c_str()); - } -} - -MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map( - const std::string & topic_name_source, const std::string & topic_name_target) -{ - return { - {"autoware_auto_control_msgs/msg/AckermannControlCommand", - [&] { - return std::static_pointer_cast( - std::make_shared(*this, topic_name_source, topic_name_target)); - }}, - {"autoware_auto_mapping_msgs/msg/HADMapBin", - [&] { - return std::static_pointer_cast( - std::make_shared(*this, topic_name_source, topic_name_target)); - }}, - {"autoware_auto_perception_msgs/msg/PredictedObjects", - [&] { - return std::static_pointer_cast( - std::make_shared(*this, topic_name_source, topic_name_target)); - }}, - {"autoware_auto_planning_msgs/msg/Trajectory", - [&] { - return std::static_pointer_cast( - std::make_shared(*this, topic_name_source, topic_name_target)); - }}, - }; -} - -void AutowareAutoMsgsAdapterNode::print_adapter_options(const MapStringAdapter & map_adapter) -{ - std::string std_options_available; - for (const auto & entry : map_adapter) { - std_options_available += entry.first + "\n"; - } - RCLCPP_INFO( - get_logger(), "Available msg_type_target options:\n%s", std_options_available.c_str()); -} - -bool AutowareAutoMsgsAdapterNode::initialize_adapter( - const MapStringAdapter & map_adapter, const std::string & msg_type_target) -{ - auto it = map_adapter.find(msg_type_target); - adapter_ = (it != map_adapter.end()) ? it->second() : nullptr; - return adapter_ != nullptr; -} - -} // namespace autoware_auto_msgs_adapter - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode) diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp deleted file mode 100644 index f506d66b1d8b0..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef ADAPTER_BASE_HPP_ -#define ADAPTER_BASE_HPP_ - -#include "adapter_base_interface.hpp" - -#include - -#include -#include - -namespace autoware_auto_msgs_adapter -{ - -template -class AdapterBase : public AdapterBaseInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(AdapterBase) - - AdapterBase( - rclcpp::Node & node, const std::string & topic_name_source, - const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) - { - pub_target_ = node.create_publisher(topic_name_target, qos); - sub_source_ = node.create_subscription( - topic_name_source, qos, std::bind(&AdapterBase::callback, this, std::placeholders::_1)); - } - -protected: - virtual TargetT convert(const SourceT & msg_source) = 0; - -private: - typename rclcpp::Publisher::SharedPtr pub_target_; - typename rclcpp::Subscription::SharedPtr sub_source_; - - void callback(const typename SourceT::SharedPtr msg_source) - { - pub_target_->publish(convert(*msg_source)); - } -}; - -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_BASE_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp deleted file mode 100644 index c02f4e03877a2..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef ADAPTER_CONTROL_HPP_ -#define ADAPTER_CONTROL_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_control_msgs::msg::Control; - -class AdapterControl -: public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterControl( - rclcpp::Node & node, const std::string & topic_name_source, - const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) - : AdapterBase(node, topic_name_source, topic_name_target, qos) - { - RCLCPP_DEBUG( - node.get_logger(), "AdapterControl is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - AckermannControlCommand convert(const Control & msg_source) override - { - autoware_auto_control_msgs::msg::AckermannControlCommand msg_auto; - msg_auto.stamp = msg_source.stamp; - - const auto & lateral = msg_source.lateral; - auto & lateral_auto = msg_auto.lateral; - lateral_auto.stamp = lateral.stamp; - lateral_auto.steering_tire_angle = lateral.steering_tire_angle; - lateral_auto.steering_tire_rotation_rate = lateral.steering_tire_rotation_rate; - - const auto & longitudinal = msg_source.longitudinal; - auto & longitudinal_auto = msg_auto.longitudinal; - longitudinal_auto.stamp = longitudinal.stamp; - longitudinal_auto.acceleration = longitudinal.acceleration; - longitudinal_auto.jerk = longitudinal.jerk; - longitudinal_auto.speed = longitudinal.velocity; - - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_CONTROL_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp deleted file mode 100644 index 8150b7d7dba08..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef ADAPTER_MAP_HPP_ -#define ADAPTER_MAP_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_map_msgs::msg::LaneletMapBin; - -class AdapterMap : public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterMap( - rclcpp::Node & node, const std::string & topic_name_source, - const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) - : AdapterBase(node, topic_name_source, topic_name_target, qos) - { - RCLCPP_DEBUG( - node.get_logger(), "AdapterMap is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - HADMapBin convert(const LaneletMapBin & msg_source) override - { - autoware_auto_mapping_msgs::msg::HADMapBin msg_auto; - msg_auto.header = msg_source.header; - msg_auto.format_version = msg_source.version_map_format; - msg_auto.map_version = msg_source.version_map; - msg_auto.map_format = 0; - msg_auto.data = msg_source.data; - - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_MAP_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp deleted file mode 100644 index 3821bbad8ce38..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef ADAPTER_PERCEPTION_HPP_ -#define ADAPTER_PERCEPTION_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using Objects_auto = autoware_auto_perception_msgs::msg::PredictedObjects; -using Objects = autoware_perception_msgs::msg::PredictedObjects; - -class AdapterPerception : public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterPerception( - rclcpp::Node & node, const std::string & topic_name_source, - const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) - : AdapterBase(node, topic_name_source, topic_name_target, qos) - { - RCLCPP_DEBUG( - node.get_logger(), "AdapterPerception is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - Objects_auto convert(const Objects & msg_source) override - { - Objects_auto msg_auto; - msg_auto.header = msg_source.header; - - autoware_auto_perception_msgs::msg::PredictedObject object_auto; - for (size_t it_of_objects = 0; it_of_objects < msg_source.objects.size(); it_of_objects++) { - // convert id and probability - object_auto.object_id = msg_source.objects[it_of_objects].object_id; - object_auto.existence_probability = msg_source.objects[it_of_objects].existence_probability; - // convert classification - autoware_auto_perception_msgs::msg::ObjectClassification classification; - for (size_t i = 0; i < msg_source.objects[it_of_objects].classification.size(); i++) { - classification.label = msg_source.objects[it_of_objects].classification[i].label; - classification.probability = - msg_source.objects[it_of_objects].classification[i].probability; - object_auto.classification.push_back(classification); - } - // convert kinematics - object_auto.kinematics.initial_pose_with_covariance = - msg_source.objects[it_of_objects].kinematics.initial_pose_with_covariance; - object_auto.kinematics.initial_twist_with_covariance = - msg_source.objects[it_of_objects].kinematics.initial_twist_with_covariance; - object_auto.kinematics.initial_acceleration_with_covariance = - msg_source.objects[it_of_objects].kinematics.initial_acceleration_with_covariance; - for (size_t j = 0; j < msg_source.objects[it_of_objects].kinematics.predicted_paths.size(); - j++) { - autoware_auto_perception_msgs::msg::PredictedPath predicted_path; - for (size_t k = 0; - k < msg_source.objects[it_of_objects].kinematics.predicted_paths[j].path.size(); k++) { - predicted_path.path.push_back( - msg_source.objects[it_of_objects].kinematics.predicted_paths[j].path[k]); - } - predicted_path.time_step = - msg_source.objects[it_of_objects].kinematics.predicted_paths[j].time_step; - predicted_path.confidence = - msg_source.objects[it_of_objects].kinematics.predicted_paths[j].confidence; - object_auto.kinematics.predicted_paths.push_back(predicted_path); - } - // convert shape - object_auto.shape.type = msg_source.objects[it_of_objects].shape.type; - object_auto.shape.footprint = msg_source.objects[it_of_objects].shape.footprint; - object_auto.shape.dimensions = msg_source.objects[it_of_objects].shape.dimensions; - - // add to objects list - msg_auto.objects.push_back(object_auto); - } - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_PERCEPTION_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp deleted file mode 100644 index 79d73bdda0575..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef ADAPTER_PLANNING_HPP_ -#define ADAPTER_PLANNING_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using TrajectoryAuto = autoware_auto_planning_msgs::msg::Trajectory; -using PointAuto = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using Trajectory = autoware_planning_msgs::msg::Trajectory; - -class AdapterPlanning : public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterPlanning( - rclcpp::Node & node, const std::string & topic_name_source, - const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) - : AdapterBase(node, topic_name_source, topic_name_target, qos) - { - RCLCPP_DEBUG( - node.get_logger(), "AdapterPlanning is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - TrajectoryAuto convert(const Trajectory & msg_source) override - { - TrajectoryAuto msg_auto; - msg_auto.header = msg_source.header; - PointAuto trajectory_point_auto; - msg_auto.points.reserve(msg_source.points.size()); - for (size_t i = 0; i < msg_source.points.size(); i++) { - trajectory_point_auto.time_from_start = msg_source.points.at(i).time_from_start; - trajectory_point_auto.pose = msg_source.points.at(i).pose; - trajectory_point_auto.longitudinal_velocity_mps = - msg_source.points.at(i).longitudinal_velocity_mps; - trajectory_point_auto.lateral_velocity_mps = msg_source.points.at(i).lateral_velocity_mps; - trajectory_point_auto.acceleration_mps2 = msg_source.points.at(i).acceleration_mps2; - trajectory_point_auto.heading_rate_rps = msg_source.points.at(i).heading_rate_rps; - trajectory_point_auto.front_wheel_angle_rad = msg_source.points.at(i).front_wheel_angle_rad; - trajectory_point_auto.rear_wheel_angle_rad = msg_source.points.at(i).rear_wheel_angle_rad; - msg_auto.points.push_back(trajectory_point_auto); - } - - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_PLANNING_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp deleted file mode 100644 index 258bf01244499..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ -#define AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ - -#include "adapter_control.hpp" -#include "adapter_map.hpp" -#include "adapter_perception.hpp" -#include "adapter_planning.hpp" - -#include - -#include -#include - -namespace autoware_auto_msgs_adapter -{ - -class AutowareAutoMsgsAdapterNode : public rclcpp::Node -{ -public: - explicit AutowareAutoMsgsAdapterNode(const rclcpp::NodeOptions & node_options); - using MapStringAdapter = std::map>; - -private: - AdapterBaseInterface::SharedPtr adapter_; - - MapStringAdapter create_adapter_map( - const std::string & topic_name_source, const std::string & topic_name_target); - - void print_adapter_options(const MapStringAdapter & map_adapter); - - bool initialize_adapter( - const MapStringAdapter & map_adapter, const std::string & msg_type_target); -}; -} // namespace autoware_auto_msgs_adapter - -#endif // AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ diff --git a/system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp b/system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp deleted file mode 100644 index 6b19ae6e30555..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -int main(int argc, char * argv[]) -{ - testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - bool result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp deleted file mode 100644 index 430b9a26b60e1..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -#include - -autoware_control_msgs::msg::Control generate_control_msg() -{ - // generate deterministic random float - std::mt19937 gen(0); - std::uniform_real_distribution<> dis(-100.0, 100.0); - auto rand_float = [&dis, &gen]() { return static_cast(dis(gen)); }; - - // generate deterministic random int - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_control_msgs::msg::Control msg_control; - msg_control.stamp = rclcpp::Time(rand_int()); - - msg_control.lateral.stamp = rclcpp::Time(rand_int()); - msg_control.lateral.steering_tire_angle = rand_float(); - msg_control.lateral.steering_tire_rotation_rate = rand_float(); - - msg_control.longitudinal.stamp = rclcpp::Time(rand_int()); - msg_control.longitudinal.velocity = rand_float(); - msg_control.longitudinal.jerk = rand_float(); - msg_control.longitudinal.acceleration = rand_float(); - return msg_control; -} - -TEST(AutowareAutoMsgsAdapter, TestMsgAckermannControlCommand) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_control_msgs/msg/AckermannControlCommand"; - const std::string topic_name_source = "topic_name_source"; - const std::string topic_name_target = "topic_name_target"; - - std::cout << "Creating the adapter node..." << std::endl; - - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("msg_type_target", msg_type_target); - node_options.append_parameter_override("topic_name_source", topic_name_source); - node_options.append_parameter_override("topic_name_target", topic_name_target); - - using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; - AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; - node_adapter = std::make_shared(node_options); - - std::cout << "Creating the subscriber node..." << std::endl; - - auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); - - bool test_completed = false; - - const auto msg_control = generate_control_msg(); - auto sub = - node_subscriber->create_subscription( - topic_name_target, rclcpp::QoS{1}, - [&msg_control, &test_completed]( - const autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg) { - EXPECT_EQ(msg->stamp, msg_control.stamp); - - EXPECT_EQ(msg->lateral.stamp, msg_control.lateral.stamp); - EXPECT_FLOAT_EQ(msg->lateral.steering_tire_angle, msg_control.lateral.steering_tire_angle); - EXPECT_FLOAT_EQ( - msg->lateral.steering_tire_rotation_rate, - msg_control.lateral.steering_tire_rotation_rate); - - EXPECT_EQ(msg->longitudinal.stamp, msg_control.longitudinal.stamp); - EXPECT_FLOAT_EQ(msg->longitudinal.speed, msg_control.longitudinal.velocity); - EXPECT_FLOAT_EQ(msg->longitudinal.acceleration, msg_control.longitudinal.acceleration); - EXPECT_FLOAT_EQ(msg->longitudinal.jerk, msg_control.longitudinal.jerk); - test_completed = true; - }); - - std::cout << "Creating the publisher node..." << std::endl; - - auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); - auto pub = node_publisher->create_publisher( - topic_name_source, rclcpp::QoS{1}); - pub->publish(msg_control); - - auto start_time = std::chrono::system_clock::now(); - auto max_test_dur = std::chrono::seconds(5); - auto timed_out = false; - - while (rclcpp::ok() && !test_completed) { - rclcpp::spin_some(node_subscriber); - rclcpp::spin_some(node_adapter); - rclcpp::spin_some(node_publisher); - rclcpp::sleep_for(std::chrono::milliseconds(50)); - if (std::chrono::system_clock::now() - start_time > max_test_dur) { - timed_out = true; - break; - } - } - - EXPECT_TRUE(test_completed); - EXPECT_FALSE(timed_out); - - // rclcpp::shutdown(); -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp deleted file mode 100644 index b312bd144f6f3..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -#include - -autoware_map_msgs::msg::LaneletMapBin generate_map_msg() -{ - // generate deterministic random int - std::mt19937 gen(0); - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_map_msgs::msg::LaneletMapBin msg_map; - msg_map.header.stamp = rclcpp::Time(rand_int()); - msg_map.header.frame_id = "test_frame"; - - msg_map.version_map_format = "1.1.1"; - msg_map.version_map = "1.0.0"; - msg_map.name_map = "florence-prato-city-center"; - msg_map.data.push_back(rand_int()); - - return msg_map; -} - -TEST(AutowareAutoMsgsAdapter, TestHADMapBin) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_mapping_msgs/msg/HADMapBin"; - const std::string topic_name_source = "topic_name_source"; - const std::string topic_name_target = "topic_name_target"; - - std::cout << "Creating the adapter node..." << std::endl; - - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("msg_type_target", msg_type_target); - node_options.append_parameter_override("topic_name_source", topic_name_source); - node_options.append_parameter_override("topic_name_target", topic_name_target); - - using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; - AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; - node_adapter = std::make_shared(node_options); - - std::cout << "Creating the subscriber node..." << std::endl; - - auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); - - bool test_completed = false; - - const auto msg_map = generate_map_msg(); - auto sub = node_subscriber->create_subscription( - topic_name_target, rclcpp::QoS{1}, - [&msg_map, &test_completed](const autoware_auto_mapping_msgs::msg::HADMapBin::SharedPtr msg) { - EXPECT_EQ(msg->header.stamp, msg_map.header.stamp); - EXPECT_EQ(msg->header.frame_id, msg_map.header.frame_id); - - EXPECT_EQ(msg->map_format, 0); - EXPECT_EQ(msg->format_version, msg_map.version_map_format); - EXPECT_EQ(msg->map_version, msg_map.version_map); - EXPECT_EQ(msg->data, msg_map.data); - - test_completed = true; - }); - - std::cout << "Creating the publisher node..." << std::endl; - - auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); - auto pub = node_publisher->create_publisher( - topic_name_source, rclcpp::QoS{1}); - pub->publish(msg_map); - - auto start_time = std::chrono::system_clock::now(); - auto max_test_dur = std::chrono::seconds(5); - auto timed_out = false; - - while (rclcpp::ok() && !test_completed) { - rclcpp::spin_some(node_subscriber); - rclcpp::spin_some(node_adapter); - rclcpp::spin_some(node_publisher); - rclcpp::sleep_for(std::chrono::milliseconds(50)); - if (std::chrono::system_clock::now() - start_time > max_test_dur) { - timed_out = true; - break; - } - } - - EXPECT_TRUE(test_completed); - EXPECT_FALSE(timed_out); - - // rclcpp::shutdown(); -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp deleted file mode 100644 index e2d44d6085c8a..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp +++ /dev/null @@ -1,149 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -#include - -autoware_planning_msgs::msg::Trajectory generate_planning_msg() -{ - using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; - // generate deterministic random int - std::mt19937 gen(0); - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_planning_msgs::msg::Trajectory msg_planning; - msg_planning.header.stamp = rclcpp::Time(rand_int()); - msg_planning.header.frame_id = "test_frame"; - - TrajectoryPoint point; - geometry_msgs::msg::Pose pose; - pose.position.x = 0.0; - pose.position.y = 0.0; - pose.position.z = 0.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - for (size_t i = 0; i < 100; i++) { - point.longitudinal_velocity_mps = 1.0; - point.time_from_start = rclcpp::Duration::from_seconds(0.0); - point.pose = pose; - point.longitudinal_velocity_mps = 20.0; - point.lateral_velocity_mps = 0.0; - point.acceleration_mps2 = 1.0; - point.heading_rate_rps = 2.0; - point.front_wheel_angle_rad = 8.0; - point.rear_wheel_angle_rad = 10.0; - - msg_planning.points.push_back(point); - } - - return msg_planning; -} - -TEST(AutowareAutoMsgsAdapter, TestTrajectory) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_planning_msgs/msg/Trajectory"; - const std::string topic_name_source = "topic_name_source"; - const std::string topic_name_target = "topic_name_target"; - - std::cout << "Creating the adapter node..." << std::endl; - - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("msg_type_target", msg_type_target); - node_options.append_parameter_override("topic_name_source", topic_name_source); - node_options.append_parameter_override("topic_name_target", topic_name_target); - - using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; - AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; - node_adapter = std::make_shared(node_options); - - std::cout << "Creating the subscriber node..." << std::endl; - - auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); - - bool test_completed = false; - - const auto msg_planning = generate_planning_msg(); - auto sub = node_subscriber->create_subscription( - topic_name_target, rclcpp::QoS{1}, - [&msg_planning, - &test_completed](const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) { - EXPECT_EQ(msg->header.stamp, msg_planning.header.stamp); - EXPECT_EQ(msg->header.frame_id, msg_planning.header.frame_id); - for (size_t i = 0; i < msg_planning.points.size(); i++) { - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.position.x, msg_planning.points.at(i).pose.position.x); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.position.y, msg_planning.points.at(i).pose.position.y); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.position.z, msg_planning.points.at(i).pose.position.z); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.x, msg_planning.points.at(i).pose.orientation.x); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.y, msg_planning.points.at(i).pose.orientation.y); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.z, msg_planning.points.at(i).pose.orientation.z); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.w, msg_planning.points.at(i).pose.orientation.w); - EXPECT_FLOAT_EQ( - msg->points.at(i).longitudinal_velocity_mps, - msg_planning.points.at(i).longitudinal_velocity_mps); - EXPECT_FLOAT_EQ( - msg->points.at(i).lateral_velocity_mps, msg_planning.points.at(i).lateral_velocity_mps); - EXPECT_FLOAT_EQ( - msg->points.at(i).acceleration_mps2, msg_planning.points.at(i).acceleration_mps2); - EXPECT_FLOAT_EQ( - msg->points.at(i).heading_rate_rps, msg_planning.points.at(i).heading_rate_rps); - EXPECT_FLOAT_EQ( - msg->points.at(i).front_wheel_angle_rad, msg_planning.points.at(i).front_wheel_angle_rad); - EXPECT_FLOAT_EQ( - msg->points.at(i).rear_wheel_angle_rad, msg_planning.points.at(i).rear_wheel_angle_rad); - } - - test_completed = true; - }); - - std::cout << "Creating the publisher node..." << std::endl; - - auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); - auto pub = node_publisher->create_publisher( - topic_name_source, rclcpp::QoS{1}); - pub->publish(msg_planning); - - auto start_time = std::chrono::system_clock::now(); - auto max_test_dur = std::chrono::seconds(5); - auto timed_out = false; - - while (rclcpp::ok() && !test_completed) { - rclcpp::spin_some(node_subscriber); - rclcpp::spin_some(node_adapter); - rclcpp::spin_some(node_publisher); - rclcpp::sleep_for(std::chrono::milliseconds(50)); - if (std::chrono::system_clock::now() - start_time > max_test_dur) { - timed_out = true; - break; - } - } - - EXPECT_TRUE(test_completed); - EXPECT_FALSE(timed_out); - - // rclcpp::shutdown(); -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp deleted file mode 100644 index a84985770059d..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp +++ /dev/null @@ -1,270 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -#include - -autoware_perception_msgs::msg::PredictedObjects generate_perception_msg() -{ - // generate deterministic random int - std::mt19937 gen(0); - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_perception_msgs::msg::PredictedObjects msg_perception; - msg_perception.header.stamp = rclcpp::Time(rand_int()); - msg_perception.header.frame_id = "test_frame"; - - autoware_perception_msgs::msg::PredictedObject obj; - // // { - unique_identifier_msgs::msg::UUID uuid_; - std::independent_bits_engine bit_eng(gen); - std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); - obj.object_id = uuid_; - obj.existence_probability = 0.5; - autoware_perception_msgs::msg::ObjectClassification obj_class; - obj_class.label = 0; - obj_class.probability = 0.5; - obj.classification.push_back(obj_class); - - // { - autoware_perception_msgs::msg::PredictedObjectKinematics kin; - kin.initial_pose_with_covariance.pose.position.x = 10; - kin.initial_pose_with_covariance.pose.position.y = 10; - kin.initial_pose_with_covariance.pose.position.z = 0; - kin.initial_pose_with_covariance.pose.orientation.x = 0; - kin.initial_pose_with_covariance.pose.orientation.y = 0; - kin.initial_pose_with_covariance.pose.orientation.z = 0; - kin.initial_pose_with_covariance.pose.orientation.w = 1; - - kin.initial_twist_with_covariance.twist.linear.x = 1; - kin.initial_twist_with_covariance.twist.linear.y = 0; - kin.initial_twist_with_covariance.twist.linear.z = 0; - kin.initial_twist_with_covariance.twist.angular.x = 0; - kin.initial_twist_with_covariance.twist.angular.y = 0; - kin.initial_twist_with_covariance.twist.angular.z = 0; - - kin.initial_acceleration_with_covariance.accel.linear.x = 0; - kin.initial_acceleration_with_covariance.accel.linear.y = 0; - kin.initial_acceleration_with_covariance.accel.linear.z = 0; - kin.initial_acceleration_with_covariance.accel.angular.x = 0; - kin.initial_acceleration_with_covariance.accel.angular.y = 0; - kin.initial_acceleration_with_covariance.accel.angular.z = 0; - - constexpr size_t path_size = 10; - kin.predicted_paths.resize(1); - kin.predicted_paths[0].path.resize(path_size); - for (size_t i = 0; i < path_size; i++) { - kin.predicted_paths[0].path[i].position.x = i; - kin.predicted_paths[0].path[i].position.y = 0; - kin.predicted_paths[0].path[i].position.z = 0; - } - obj.kinematics = kin; - // } - // { - autoware_perception_msgs::msg::Shape s; - s.type = 1; - geometry_msgs::msg::Point32 p; - p.x = 9.0f; - p.y = 11.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - p.x = 11.0f; - p.y = 11.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - p.x = 11.0f; - p.y = 9.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - p.x = 9.0f; - p.y = 9.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - - s.dimensions.x = 2; - s.dimensions.y = 2; - s.dimensions.z = 2; - - obj.shape = s; - // } - // } - - msg_perception.objects.push_back(obj); - return msg_perception; -} - -TEST(AutowareAutoMsgsAdapter, TestPredictedObjects) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_perception_msgs/msg/PredictedObjects"; - const std::string topic_name_source = "topic_name_source"; - const std::string topic_name_target = "topic_name_target"; - - std::cout << "Creating the adapter node..." << std::endl; - - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("msg_type_target", msg_type_target); - node_options.append_parameter_override("topic_name_source", topic_name_source); - node_options.append_parameter_override("topic_name_target", topic_name_target); - - using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; - AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; - node_adapter = std::make_shared(node_options); - - std::cout << "Creating the subscriber node..." << std::endl; - - auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); - - bool test_completed = false; - - const auto msg_perception = generate_perception_msg(); - auto sub = node_subscriber->create_subscription< - autoware_auto_perception_msgs::msg::PredictedObjects>( - topic_name_target, rclcpp::QoS{1}, - [&msg_perception, - &test_completed](const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr msg) { - EXPECT_EQ(msg->header.stamp, msg_perception.header.stamp); - EXPECT_EQ(msg->header.frame_id, msg_perception.header.frame_id); - EXPECT_EQ(msg->objects[0].object_id.uuid, msg_perception.objects[0].object_id.uuid); - EXPECT_FLOAT_EQ( - msg->objects[0].existence_probability, msg_perception.objects[0].existence_probability); - EXPECT_EQ( - msg->objects[0].classification[0].label, msg_perception.objects[0].classification[0].label); - EXPECT_FLOAT_EQ( - msg->objects[0].classification[0].probability, - msg_perception.objects[0].classification[0].probability); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.x, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.y, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.z, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.x, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.y, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.z, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.w, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.w); - - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.x, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.y, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.z, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.x, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.y, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.z, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.z); - - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.x, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.y, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.z, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.x, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.y, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.z, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.z); - - for (size_t i = 0; i < msg->objects[0].kinematics.predicted_paths[0].path.size(); i++) { - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.predicted_paths[0].path[i].position.x, - msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.predicted_paths[0].path[i].position.y, - msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.predicted_paths[0].path[i].position.z, - msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.z); - } - - EXPECT_EQ(msg->objects[0].shape.type, msg_perception.objects[0].shape.type); - for (size_t i = 0; i < msg_perception.objects[0].shape.footprint.points.size(); i++) { - EXPECT_FLOAT_EQ( - msg->objects[0].shape.footprint.points[i].x, - msg_perception.objects[0].shape.footprint.points[i].x); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.footprint.points[i].y, - msg_perception.objects[0].shape.footprint.points[i].y); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.footprint.points[i].z, - msg_perception.objects[0].shape.footprint.points[i].z); - } - EXPECT_FLOAT_EQ( - msg->objects[0].shape.dimensions.x, msg_perception.objects[0].shape.dimensions.x); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.dimensions.y, msg_perception.objects[0].shape.dimensions.y); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.dimensions.z, msg_perception.objects[0].shape.dimensions.z); - - test_completed = true; - }); - - std::cout << "Creating the publisher node..." << std::endl; - - auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); - auto pub = node_publisher->create_publisher( - topic_name_source, rclcpp::QoS{1}); - pub->publish(msg_perception); - - auto start_time = std::chrono::system_clock::now(); - auto max_test_dur = std::chrono::seconds(5); - auto timed_out = false; - - while (rclcpp::ok() && !test_completed) { - rclcpp::spin_some(node_subscriber); - rclcpp::spin_some(node_adapter); - rclcpp::spin_some(node_publisher); - rclcpp::sleep_for(std::chrono::milliseconds(50)); - if (std::chrono::system_clock::now() - start_time > max_test_dur) { - timed_out = true; - break; - } - } - - EXPECT_TRUE(test_completed); - EXPECT_FALSE(timed_out); - - // rclcpp::shutdown(); -} diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index 098aa0e56b1ae..3845a96108d2b 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -4,7 +4,7 @@ args: node_name_suffix: vector_map topic: /map/vector_map - topic_type: autoware_auto_mapping_msgs/msg/HADMapBin + topic_type: autoware_map_msgs/msg/LaneletMapBin best_effort: false transient_local: true warn_rate: 0.0 @@ -69,7 +69,7 @@ args: node_name_suffix: object_recognition_objects topic: /perception/object_recognition/objects - topic_type: autoware_auto_perception_msgs/msg/PredictedObjects + topic_type: autoware_perception_msgs/msg/PredictedObjects best_effort: false transient_local: false warn_rate: 5.0 @@ -82,7 +82,7 @@ args: node_name_suffix: traffic_light_recognition_traffic_signals topic: /perception/traffic_light_recognition/traffic_signals - topic_type: autoware_perception_msgs/msg/TrafficSignalArray + topic_type: autoware_perception_msgs/msg/TrafficLightGroupArray best_effort: false transient_local: false warn_rate: 5.0 @@ -108,7 +108,7 @@ args: node_name_suffix: scenario_planning_trajectory topic: /planning/scenario_planning/trajectory - topic_type: autoware_auto_planning_msgs/msg/Trajectory + topic_type: autoware_planning_msgs/msg/Trajectory best_effort: false transient_local: false warn_rate: 5.0 @@ -121,7 +121,7 @@ args: node_name_suffix: trajectory_follower_control_cmd topic: /control/trajectory_follower/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 @@ -134,7 +134,7 @@ args: node_name_suffix: control_command_control_cmd topic: /control/command/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 @@ -147,7 +147,7 @@ args: node_name_suffix: vehicle_status_velocity_status topic: /vehicle/status/velocity_status - topic_type: autoware_auto_vehicle_msgs/msg/VelocityReport + topic_type: autoware_vehicle_msgs/msg/VelocityReport best_effort: false transient_local: false warn_rate: 5.0 @@ -160,7 +160,7 @@ args: node_name_suffix: vehicle_status_steering_status topic: /vehicle/status/steering_status - topic_type: autoware_auto_vehicle_msgs/msg/SteeringReport + topic_type: autoware_vehicle_msgs/msg/SteeringReport best_effort: false transient_local: false warn_rate: 5.0 @@ -173,7 +173,7 @@ args: node_name_suffix: system_emergency_control_cmd topic: /system/emergency/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 982112189412e..799a73508a061 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -5,7 +5,9 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED + src/diagnostics.cpp src/fail_safe.cpp + src/heartbeat.cpp src/interface.cpp src/localization.cpp src/motion.cpp @@ -16,13 +18,16 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/vehicle.cpp src/vehicle_info.cpp src/vehicle_door.cpp + src/utils/localization_conversion.cpp src/utils/route_conversion.cpp src/compatibility/autoware_state.cpp ) rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::AutowareStateNode" + "default_ad_api::DiagnosticsNode" "default_ad_api::FailSafeNode" + "default_ad_api::HeartbeatNode" "default_ad_api::InterfaceNode" "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index cf8fbe7d001bf..ca1575a528db2 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -42,7 +42,9 @@ def get_default_config(): def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), + create_api_node("diagnostics", "DiagnosticsNode"), create_api_node("fail_safe", "FailSafeNode"), + create_api_node("heartbeat", "HeartbeatNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode"), diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 4e691f7ab3192..77276c7acb0e3 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -15,12 +15,12 @@ autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils + diagnostic_graph_utils geographic_msgs geography_utils motion_utils diff --git a/system/default_ad_api/src/compatibility/autoware_state.hpp b/system/default_ad_api/src/compatibility/autoware_state.hpp index 4cd6233f5df0a..eff74728efcb0 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.hpp +++ b/system/default_ad_api/src/compatibility/autoware_state.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -44,7 +44,7 @@ class AutowareStateNode : public rclcpp::Node Sub sub_routing_; Sub sub_operation_mode_; - using AutowareState = autoware_auto_system_msgs::msg::AutowareState; + using AutowareState = autoware_system_msgs::msg::AutowareState; using LocalizationState = autoware_ad_api::localization::InitializationState::Message; using RoutingState = autoware_ad_api::routing::RouteState::Message; using OperationModeState = autoware_ad_api::operation_mode::OperationModeState::Message; diff --git a/system/default_ad_api/src/diagnostics.cpp b/system/default_ad_api/src/diagnostics.cpp new file mode 100644 index 0000000000000..6eaaa5bf614a6 --- /dev/null +++ b/system/default_ad_api/src/diagnostics.cpp @@ -0,0 +1,81 @@ +// Copyright 2024 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 "diagnostics.hpp" + +#include +#include + +namespace default_ad_api +{ + +DiagnosticsNode::DiagnosticsNode(const rclcpp::NodeOptions & options) : Node("diagnostics", options) +{ + using std::placeholders::_1; + + pub_struct_ = create_publisher( + "/api/system/diagnostics/struct", rclcpp::QoS(1).transient_local()); + pub_status_ = create_publisher( + "/api/system/diagnostics/status", rclcpp::QoS(1).best_effort()); + + sub_graph_.register_create_callback(std::bind(&DiagnosticsNode::on_create, this, _1)); + sub_graph_.register_update_callback(std::bind(&DiagnosticsNode::on_update, this, _1)); + sub_graph_.subscribe(*this, 10); +} +void DiagnosticsNode::on_create(DiagGraph::ConstSharedPtr graph) +{ + const auto & units = graph->units(); + const auto & links = graph->links(); + + std::unordered_map unit_indices_; + for (size_t i = 0; i < units.size(); ++i) { + unit_indices_[units[i]] = i; + } + + autoware_adapi_v1_msgs::msg::DiagGraphStruct msg; + msg.stamp = graph->created_stamp(); + msg.id = graph->id(); + msg.nodes.reserve(units.size()); + msg.links.reserve(links.size()); + for (const auto & unit : units) { + msg.nodes.emplace_back(); + msg.nodes.back().path = unit->path(); + } + for (const auto & link : links) { + msg.links.emplace_back(); + msg.links.back().parent = unit_indices_.at(link->parent()); + msg.links.back().child = unit_indices_.at(link->child()); + } + pub_struct_->publish(msg); +} + +void DiagnosticsNode::on_update(DiagGraph::ConstSharedPtr graph) +{ + const auto & units = graph->units(); + + autoware_adapi_v1_msgs::msg::DiagGraphStatus msg; + msg.stamp = graph->updated_stamp(); + msg.id = graph->id(); + msg.nodes.reserve(units.size()); + for (const auto & unit : units) { + msg.nodes.emplace_back(); + msg.nodes.back().level = unit->level(); + } + pub_status_->publish(msg); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::DiagnosticsNode) diff --git a/system/default_ad_api/src/diagnostics.hpp b/system/default_ad_api/src/diagnostics.hpp new file mode 100644 index 0000000000000..302ffe39159df --- /dev/null +++ b/system/default_ad_api/src/diagnostics.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 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. + +#ifndef DIAGNOSTICS_HPP_ +#define DIAGNOSTICS_HPP_ + +#include "diagnostic_graph_utils/subscription.hpp" + +#include + +#include +#include + +namespace default_ad_api +{ + +class DiagnosticsNode : public rclcpp::Node +{ +public: + explicit DiagnosticsNode(const rclcpp::NodeOptions & options); + +private: + using DiagGraph = diagnostic_graph_utils::DiagGraph; + using DiagUnit = diagnostic_graph_utils::DiagUnit; + using DiagLink = diagnostic_graph_utils::DiagLink; + void on_create(DiagGraph::ConstSharedPtr graph); + void on_update(DiagGraph::ConstSharedPtr graph); + rclcpp::Publisher::SharedPtr pub_struct_; + rclcpp::Publisher::SharedPtr pub_status_; + diagnostic_graph_utils::DiagGraphSubscription sub_graph_; +}; + +} // namespace default_ad_api + +#endif // DIAGNOSTICS_HPP_ diff --git a/system/default_ad_api/src/heartbeat.cpp b/system/default_ad_api/src/heartbeat.cpp new file mode 100644 index 0000000000000..4550302bb8bae --- /dev/null +++ b/system/default_ad_api/src/heartbeat.cpp @@ -0,0 +1,42 @@ +// Copyright 2024 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 "heartbeat.hpp" + +#include + +namespace default_ad_api +{ + +HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartbeat", options) +{ + // Move this function so that the timer no longer holds it as a reference. + const auto on_timer = [this]() { + autoware_ad_api::system::Heartbeat::Message heartbeat; + heartbeat.stamp = now(); + heartbeat.seq = ++sequence_; // Wraps at 65535. + pub_->publish(heartbeat); + }; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(pub_); + + const auto period = rclcpp::Rate(10.0).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, std::move(on_timer)); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::HeartbeatNode) diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp b/system/default_ad_api/src/heartbeat.hpp similarity index 53% rename from system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp rename to system/default_ad_api/src/heartbeat.hpp index 606993332fc4a..d922b88489639 100644 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp +++ b/system/default_ad_api/src/heartbeat.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 The Autoware Foundation +// Copyright 2024 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. @@ -11,24 +11,30 @@ // 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 ADAPTER_BASE_INTERFACE_HPP_ -#define ADAPTER_BASE_INTERFACE_HPP_ +#ifndef HEARTBEAT_HPP_ +#define HEARTBEAT_HPP_ + +#include #include -#include +// This file should be included after messages. +#include "utils/types.hpp" -namespace autoware_auto_msgs_adapter +namespace default_ad_api { -class AdapterBaseInterface +class HeartbeatNode : public rclcpp::Node { public: - RCLCPP_SHARED_PTR_DEFINITIONS(AdapterBaseInterface) + explicit HeartbeatNode(const rclcpp::NodeOptions & options); - virtual ~AdapterBaseInterface() = default; +private: + rclcpp::TimerBase::SharedPtr timer_; + Pub pub_; + uint16_t sequence_ = 0; }; -} // namespace autoware_auto_msgs_adapter +} // namespace default_ad_api -#endif // ADAPTER_BASE_INTERFACE_HPP_ +#endif // HEARTBEAT_HPP_ diff --git a/system/default_ad_api/src/interface.cpp b/system/default_ad_api/src/interface.cpp index 506a4aa8bf5eb..d53d12afcc499 100644 --- a/system/default_ad_api/src/interface.cpp +++ b/system/default_ad_api/src/interface.cpp @@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf { const auto on_interface_version = [](auto, auto res) { res->major = 1; - res->minor = 2; + res->minor = 3; res->patch = 0; }; diff --git a/system/default_ad_api/src/localization.cpp b/system/default_ad_api/src/localization.cpp index 468e02b37644f..97544a7868d9c 100644 --- a/system/default_ad_api/src/localization.cpp +++ b/system/default_ad_api/src/localization.cpp @@ -14,6 +14,8 @@ #include "localization.hpp" +#include "utils/localization_conversion.hpp" + namespace default_ad_api { @@ -23,7 +25,15 @@ LocalizationNode::LocalizationNode(const rclcpp::NodeOptions & options) const auto adaptor = component_interface_utils::NodeAdaptor(this); group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); adaptor.relay_message(pub_state_, sub_state_); - adaptor.relay_service(cli_initialize_, srv_initialize_, group_cli_); + adaptor.init_cli(cli_initialize_); + adaptor.init_srv(srv_initialize_, this, &LocalizationNode::on_initialize, group_cli_); +} + +void LocalizationNode::on_initialize( + const autoware_ad_api::localization::Initialize::Service::Request::SharedPtr req, + const autoware_ad_api::localization::Initialize::Service::Response::SharedPtr res) +{ + res->status = localization_conversion::convert_call(cli_initialize_, req); } } // namespace default_ad_api diff --git a/system/default_ad_api/src/localization.hpp b/system/default_ad_api/src/localization.hpp index 71517c4c6c769..a24e2110fabd1 100644 --- a/system/default_ad_api/src/localization.hpp +++ b/system/default_ad_api/src/localization.hpp @@ -36,6 +36,10 @@ class LocalizationNode : public rclcpp::Node Pub pub_state_; Cli cli_initialize_; Sub sub_state_; + + void on_initialize( + const autoware_ad_api::localization::Initialize::Service::Request::SharedPtr req, + const autoware_ad_api::localization::Initialize::Service::Response::SharedPtr res); }; } // namespace default_ad_api diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index 829585ed4b8b4..d4b430ecd0579 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -39,18 +39,15 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) adaptor.init_cli(cli_mode_, group_cli_); adaptor.init_cli(cli_control_, group_cli_); - const std::vector module_names = { - "sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system", + const auto name = "/system/operation_mode/availability"; + const auto qos = rclcpp::QoS(1); + const auto callback = [this](const OperationModeAvailability::ConstSharedPtr msg) { + mode_available_[OperationModeState::Message::STOP] = msg->stop; + mode_available_[OperationModeState::Message::AUTONOMOUS] = msg->autonomous; + mode_available_[OperationModeState::Message::LOCAL] = msg->local; + mode_available_[OperationModeState::Message::REMOTE] = msg->remote; }; - - for (size_t i = 0; i < module_names.size(); ++i) { - const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i]; - const auto qos = rclcpp::QoS(1).transient_local(); - const auto callback = [this, i, module_names](const ModeChangeAvailable::ConstSharedPtr msg) { - module_states_[module_names[i]] = msg->available; - }; - sub_module_states_.push_back(create_subscription(name, qos, callback)); - } + sub_availability_ = create_subscription(name, qos, callback); timer_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this)); @@ -60,8 +57,8 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) mode_available_[OperationModeState::Message::UNKNOWN] = false; mode_available_[OperationModeState::Message::STOP] = true; mode_available_[OperationModeState::Message::AUTONOMOUS] = false; - mode_available_[OperationModeState::Message::LOCAL] = true; - mode_available_[OperationModeState::Message::REMOTE] = true; + mode_available_[OperationModeState::Message::LOCAL] = false; + mode_available_[OperationModeState::Message::REMOTE] = false; } template @@ -135,23 +132,6 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP void OperationModeNode::on_timer() { - bool autonomous_available = true; - std::string unhealthy_components = ""; - for (const auto & state : module_states_) { - if (!state.second) { - unhealthy_components += unhealthy_components.empty() ? state.first : ", " + state.first; - } - autonomous_available &= state.second; - } - mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available; - - if (!unhealthy_components.empty()) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 3000, - "%s component state is unhealthy. Autonomous is not available.", - unhealthy_components.c_str()); - } - update_state(); } diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 1830b7041b566..7cd609b5eb852 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -25,7 +25,7 @@ #include // TODO(Takagi, Isamu): define interface -#include +#include // This file should be included after messages. #include "utils/types.hpp" @@ -47,7 +47,7 @@ class OperationModeNode : public rclcpp::Node using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; - using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable; + using OperationModeAvailability = tier4_system_msgs::msg::OperationModeAvailability; OperationModeState::Message curr_state_; OperationModeState::Message prev_state_; @@ -65,9 +65,7 @@ class OperationModeNode : public rclcpp::Node Sub sub_state_; Cli cli_mode_; Cli cli_control_; - - std::unordered_map module_states_; - std::vector::SharedPtr> sub_module_states_; + rclcpp::Subscription::SharedPtr sub_availability_; void on_change_to_stop( const ChangeToStop::Service::Request::SharedPtr req, diff --git a/system/default_ad_api/src/perception.cpp b/system/default_ad_api/src/perception.cpp index 61d09444c70a4..794bbf141ca8b 100644 --- a/system/default_ad_api/src/perception.cpp +++ b/system/default_ad_api/src/perception.cpp @@ -24,7 +24,7 @@ using ObjectClassification = autoware_adapi_v1_msgs::msg::ObjectClassification; using DynamicObject = autoware_adapi_v1_msgs::msg::DynamicObject; using DynamicObjectPath = autoware_adapi_v1_msgs::msg::DynamicObjectPath; using API_Shape = shape_msgs::msg::SolidPrimitive; -using Shape = autoware_auto_perception_msgs::msg::Shape; +using Shape = autoware_perception_msgs::msg::Shape; std::unordered_map shape_type_ = { {Shape::BOUNDING_BOX, API_Shape::BOX}, diff --git a/system/default_ad_api/src/perception.hpp b/system/default_ad_api/src/perception.hpp index d1c71fdb08025..ff3e4801b07ac 100644 --- a/system/default_ad_api/src/perception.hpp +++ b/system/default_ad_api/src/perception.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 887e1167a7c3e..c40bfac40a135 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -85,7 +85,8 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning "/planning/velocity_factors/surround_obstacle", "/planning/velocity_factors/traffic_light", "/planning/velocity_factors/virtual_traffic_light", - "/planning/velocity_factors/walkway"}; + "/planning/velocity_factors/walkway", + "/planning/velocity_factors/motion_velocity_planner"}; std::vector steering_factor_topics = { "/planning/steering_factor/avoidance", "/planning/steering_factor/intersection", diff --git a/system/default_ad_api/src/utils/localization_conversion.cpp b/system/default_ad_api/src/utils/localization_conversion.cpp new file mode 100644 index 0000000000000..3ddf259a4a590 --- /dev/null +++ b/system/default_ad_api/src/utils/localization_conversion.cpp @@ -0,0 +1,40 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "localization_conversion.hpp" + +#include + +namespace default_ad_api::localization_conversion +{ + +InternalInitializeRequest convert_request(const ExternalInitializeRequest & external) +{ + auto internal = std::make_shared(); + internal->pose_with_covariance = external->pose; + internal->method = tier4_localization_msgs::srv::InitializeLocalization::Request::AUTO; + return internal; +} + +ExternalResponse convert_response(const InternalResponse & internal) +{ + // TODO(Takagi, Isamu): check error code correspondence + ExternalResponse external; + external.success = internal.success; + external.code = internal.code; + external.message = internal.message; + return external; +} + +} // namespace default_ad_api::localization_conversion diff --git a/system/default_ad_api/src/utils/localization_conversion.hpp b/system/default_ad_api/src/utils/localization_conversion.hpp new file mode 100644 index 0000000000000..85b6e81e6cd91 --- /dev/null +++ b/system/default_ad_api/src/utils/localization_conversion.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS__LOCALIZATION_CONVERSION_HPP_ +#define UTILS__LOCALIZATION_CONVERSION_HPP_ + +#include + +#include +#include + +namespace default_ad_api::localization_conversion +{ + +using ExternalInitializeRequest = + autoware_adapi_v1_msgs::srv::InitializeLocalization::Request::SharedPtr; +using InternalInitializeRequest = + tier4_localization_msgs::srv::InitializeLocalization::Request::SharedPtr; +InternalInitializeRequest convert_request(const ExternalInitializeRequest & external); + +using ExternalResponse = autoware_adapi_v1_msgs::msg::ResponseStatus; +using InternalResponse = autoware_common_msgs::msg::ResponseStatus; +ExternalResponse convert_response(const InternalResponse & internal); + +template +ExternalResponse convert_call(ClientT & client, RequestT & req) +{ + return convert_response(client->call(convert_request(req))->status); +} + +} // namespace default_ad_api::localization_conversion + +#endif // UTILS__LOCALIZATION_CONVERSION_HPP_ diff --git a/system/default_ad_api/test/node/interface_version.py b/system/default_ad_api/test/node/interface_version.py index ccfa3e1101185..63a79e8722be3 100644 --- a/system/default_ad_api/test/node/interface_version.py +++ b/system/default_ad_api/test/node/interface_version.py @@ -30,7 +30,7 @@ if response.major != 1: exit(1) -if response.minor != 2: +if response.minor != 3: exit(1) if response.patch != 0: exit(1) diff --git a/system/diagnostic_graph_utils/CMakeLists.txt b/system/diagnostic_graph_utils/CMakeLists.txt index 48af5e8fc304f..b68e7bedb57ed 100644 --- a/system/diagnostic_graph_utils/CMakeLists.txt +++ b/system/diagnostic_graph_utils/CMakeLists.txt @@ -9,12 +9,19 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/lib/subscription.cpp ) -ament_auto_add_executable(dump +ament_auto_add_library(${PROJECT_NAME}_tools SHARED src/node/dump.cpp + src/node/converter.cpp ) -ament_auto_add_executable(converter - src/node/converter.cpp +rclcpp_components_register_node(${PROJECT_NAME}_tools + PLUGIN "diagnostic_graph_utils::DumpNode" + EXECUTABLE dump_node +) + +rclcpp_components_register_node(${PROJECT_NAME}_tools + PLUGIN "diagnostic_graph_utils::ConverterNode" + EXECUTABLE converter_node ) ament_auto_package() diff --git a/system/diagnostic_graph_utils/doc/node/converter.md b/system/diagnostic_graph_utils/doc/node/converter.md index c3db547167474..407a99c87f73e 100644 --- a/system/diagnostic_graph_utils/doc/node/converter.md +++ b/system/diagnostic_graph_utils/doc/node/converter.md @@ -5,7 +5,7 @@ This tool converts `/diagnostics_graph` to `/diagnostics_array` so it can be rea ## Usage ```bash -ros2 run diagnostic_graph_utils converter +ros2 run diagnostic_graph_utils converter_node ``` ## Examples @@ -19,7 +19,7 @@ ros2 launch diagnostic_graph_aggregator example-main.launch.xml Terminal 2: ```bash -ros2 run diagnostic_graph_utils converter +ros2 run diagnostic_graph_utils converter_node ``` Terminal 3: diff --git a/system/diagnostic_graph_utils/doc/node/dump.md b/system/diagnostic_graph_utils/doc/node/dump.md index 090e9679676b6..c76bb85ed75cb 100644 --- a/system/diagnostic_graph_utils/doc/node/dump.md +++ b/system/diagnostic_graph_utils/doc/node/dump.md @@ -5,7 +5,7 @@ This tool displays `/diagnostics_graph` in table format. ## Usage ```bash -ros2 run diagnostic_graph_utils dump +ros2 run diagnostic_graph_utils dump_node ``` ## Examples @@ -19,7 +19,7 @@ ros2 launch diagnostic_graph_aggregator example-main.launch.xml Terminal 2: ```bash -ros2 run diagnostic_graph_utils dump +ros2 run diagnostic_graph_utils dump_node ``` Output: diff --git a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp b/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp index b2c6fc752e463..c5b386dbe2c86 100644 --- a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp +++ b/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp @@ -55,12 +55,20 @@ class DiagLink public: using DiagLinkStruct = tier4_system_msgs::msg::DiagLinkStruct; using DiagLinkStatus = tier4_system_msgs::msg::DiagLinkStatus; - explicit DiagLink(const DiagLinkStruct & msg) : struct_(msg) {} + DiagLink(const DiagLinkStruct & msg, DiagUnit * parent, DiagUnit * child) : struct_(msg) + { + parent_ = parent; + child_ = child; + } void update(const DiagLinkStatus & msg) { status_ = msg; } + DiagUnit * parent() const { return parent_; } + DiagUnit * child() const { return child_; } private: DiagLinkStruct struct_; DiagLinkStatus status_; + DiagUnit * parent_; + DiagUnit * child_; }; class DiagNode : public DiagUnit @@ -114,6 +122,7 @@ class DiagGraph bool update(const DiagGraphStatus & msg); rclcpp::Time created_stamp() const { return created_stamp_; } rclcpp::Time updated_stamp() const { return updated_stamp_; } + std::string id() const { return id_; } std::vector units() const; std::vector nodes() const; std::vector diags() const; diff --git a/system/diagnostic_graph_utils/package.xml b/system/diagnostic_graph_utils/package.xml index 42d44efb6697e..c5a70363bfecb 100644 --- a/system/diagnostic_graph_utils/package.xml +++ b/system/diagnostic_graph_utils/package.xml @@ -12,6 +12,7 @@ diagnostic_msgs rclcpp + rclcpp_components tier4_system_msgs ament_lint_auto diff --git a/system/diagnostic_graph_utils/src/lib/graph.cpp b/system/diagnostic_graph_utils/src/lib/graph.cpp index f1177c1047bdb..bd4c215a346ed 100644 --- a/system/diagnostic_graph_utils/src/lib/graph.cpp +++ b/system/diagnostic_graph_utils/src/lib/graph.cpp @@ -54,7 +54,7 @@ void DiagGraph::create(const DiagGraphStruct & msg) for (const auto & data : msg.links) { DiagNode * parent = nodes_.at(data.parent).get(); DiagUnit * child = get_child(data.is_leaf, data.child); - const auto link = links_.emplace_back(std::make_unique(data)).get(); + const auto link = links_.emplace_back(std::make_unique(data, parent, child)).get(); parent->add_child({link, child}); } } diff --git a/system/diagnostic_graph_utils/src/node/converter.cpp b/system/diagnostic_graph_utils/src/node/converter.cpp index e9e40fa0a0756..159cc6e0c3cab 100644 --- a/system/diagnostic_graph_utils/src/node/converter.cpp +++ b/system/diagnostic_graph_utils/src/node/converter.cpp @@ -19,7 +19,7 @@ namespace diagnostic_graph_utils { -ConverterNode::ConverterNode() : Node("converter") +ConverterNode::ConverterNode(const rclcpp::NodeOptions & options) : Node("converter", options) { using std::placeholders::_1; pub_array_ = create_publisher("/diagnostics_array", rclcpp::QoS(1)); @@ -40,14 +40,5 @@ void ConverterNode::on_update(DiagGraph::ConstSharedPtr graph) } // namespace diagnostic_graph_utils -int main(int argc, char ** argv) -{ - using diagnostic_graph_utils::ConverterNode; - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::ConverterNode) diff --git a/system/diagnostic_graph_utils/src/node/converter.hpp b/system/diagnostic_graph_utils/src/node/converter.hpp index 09ce62d7293ec..19364a8ff8240 100644 --- a/system/diagnostic_graph_utils/src/node/converter.hpp +++ b/system/diagnostic_graph_utils/src/node/converter.hpp @@ -28,7 +28,7 @@ namespace diagnostic_graph_utils class ConverterNode : public rclcpp::Node { public: - ConverterNode(); + explicit ConverterNode(const rclcpp::NodeOptions & options); private: using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; diff --git a/system/diagnostic_graph_utils/src/node/dump.cpp b/system/diagnostic_graph_utils/src/node/dump.cpp index 5fa4922dec1a5..8456a92cbaa9b 100644 --- a/system/diagnostic_graph_utils/src/node/dump.cpp +++ b/system/diagnostic_graph_utils/src/node/dump.cpp @@ -24,7 +24,7 @@ namespace diagnostic_graph_utils { -DumpNode::DumpNode() : Node("dump") +DumpNode::DumpNode(const rclcpp::NodeOptions & options) : Node("dump", options) { using std::placeholders::_1; sub_graph_.register_create_callback(std::bind(&DumpNode::on_create, this, _1)); @@ -132,14 +132,5 @@ void DumpNode::on_update(DiagGraph::ConstSharedPtr graph) } // namespace diagnostic_graph_utils -int main(int argc, char ** argv) -{ - using diagnostic_graph_utils::DumpNode; - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::DumpNode) diff --git a/system/diagnostic_graph_utils/src/node/dump.hpp b/system/diagnostic_graph_utils/src/node/dump.hpp index e37a1ea971bf5..c990fb77a53db 100644 --- a/system/diagnostic_graph_utils/src/node/dump.hpp +++ b/system/diagnostic_graph_utils/src/node/dump.hpp @@ -28,7 +28,7 @@ namespace diagnostic_graph_utils class DumpNode : public rclcpp::Node { public: - DumpNode(); + explicit DumpNode(const rclcpp::NodeOptions & options); private: void on_create(DiagGraph::ConstSharedPtr graph); diff --git a/system/dummy_diag_publisher/CMakeLists.txt b/system/dummy_diag_publisher/CMakeLists.txt index cecb317bf9c34..794e7d35e1194 100644 --- a/system/dummy_diag_publisher/CMakeLists.txt +++ b/system/dummy_diag_publisher/CMakeLists.txt @@ -4,11 +4,15 @@ project(dummy_diag_publisher) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(${PROJECT_NAME} - src/dummy_diag_publisher_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/dummy_diag_publisher_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "DummyDiagPublisher" + EXECUTABLE ${PROJECT_NAME}_node +) + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp b/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp index 8398c15b8e8f6..071e665ece6ec 100644 --- a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp +++ b/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp @@ -35,7 +35,7 @@ struct DiagConfig class DummyDiagPublisher : public rclcpp::Node { public: - DummyDiagPublisher(); + explicit DummyDiagPublisher(const rclcpp::NodeOptions & options); private: enum Status { diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml index 8e71ce37543c2..9d9193fb5f7a7 100644 --- a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml +++ b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp index cfe1692c91df9..9abf325e62833 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -245,12 +245,14 @@ void DummyDiagPublisher::onTimer() updater_.force_update(); } -DummyDiagPublisher::DummyDiagPublisher() -: Node( - "dummy_diag_publisher", rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)), - updater_(this) +rclcpp::NodeOptions override_options(rclcpp::NodeOptions options) +{ + return options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides( + true); +} + +DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options) +: Node("dummy_diag_publisher", override_options(options)), updater_(this) { // Parameter @@ -277,3 +279,6 @@ DummyDiagPublisher::DummyDiagPublisher() timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&DummyDiagPublisher::onTimer, this)); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(DummyDiagPublisher) diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp deleted file mode 100644 index a532e8d1c6d01..0000000000000 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "dummy_diag_publisher/dummy_diag_publisher_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/emergency_handler/README.md b/system/emergency_handler/README.md index 3d658dbb4586f..f34ebcf8d9b9a 100644 --- a/system/emergency_handler/README.md +++ b/system/emergency_handler/README.md @@ -14,24 +14,24 @@ Emergency Handler is a node to select proper MRM from from system failure state ### Input -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------------------------------- | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | -| `/control/vehicle_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Used as reference when generate Emergency Control Command | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | -| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | -| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | +| Name | Type | Description | +| ----------------------------------------- | ------------------------------------------------ | ----------------------------------------------------------------------------- | +| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | +| `/control/vehicle_cmd` | `autoware_control_msgs::msg::Control` | Used as reference when generate Emergency Control Command | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | +| `/vehicle/status/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | +| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | +| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | ### Output -| Name | Type | Description | -| ------------------------------------------ | ------------------------------------------------------ | ----------------------------------------------------- | -| `/system/emergency/shift_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | -| `/system/emergency/hazard_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | -| `/api/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | -| `/system/api/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | -| `/system/api/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | +| Name | Type | Description | +| ------------------------------------------ | ------------------------------------------------- | ----------------------------------------------------- | +| `/system/emergency/shift_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | +| `/system/emergency/hazard_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | +| `/api/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | +| `/system/api/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | +| `/system/api/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | ## Parameters 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 a78b35be26daf..5ff269705456d 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -21,11 +21,11 @@ // Autoware #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -57,48 +57,43 @@ class EmergencyHandler : public rclcpp::Node private: // Subscribers - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_hazard_status_stamped_; - rclcpp::Subscription::SharedPtr - sub_prev_control_command_; + rclcpp::Subscription::SharedPtr sub_prev_control_command_; rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; + rclcpp::Subscription::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_mrm_comfortable_stop_status_; rclcpp::Subscription::SharedPtr sub_mrm_emergency_stop_status_; - autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; - autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_; + 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_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + 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_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); - void onPrevControlCommand( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + 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_auto_vehicle_msgs::msg::ControlModeReport::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_; + rclcpp::Publisher::SharedPtr pub_control_command_; // rclcpp::Publisher::SharedPtr pub_shift_; // rclcpp::Publisher::SharedPtr pub_turn_signal_; - rclcpp::Publisher::SharedPtr - pub_hazard_cmd_; - rclcpp::Publisher::SharedPtr pub_gear_cmd_; + rclcpp::Publisher::SharedPtr pub_hazard_cmd_; + rclcpp::Publisher::SharedPtr pub_gear_cmd_; - autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); - autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg(); + autoware_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); + autoware_vehicle_msgs::msg::GearCommand createGearCmdMsg(); void publishControlCommands(); rclcpp::Publisher::SharedPtr pub_mrm_state_; diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index 97344f9b8c594..aa2090c86edb8 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -13,9 +13,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_system_msgs + autoware_vehicle_msgs nav_msgs rclcpp rclcpp_components 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 2770a14691d95..b96cb0d0549f9 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -31,18 +31,16 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) using std::placeholders::_1; // Subscriber - 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_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( + 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}, @@ -52,12 +50,12 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1)); // Publisher - pub_control_command_ = create_publisher( + pub_control_command_ = create_publisher( "~/output/control_command", rclcpp::QoS{1}); - pub_hazard_cmd_ = create_publisher( + pub_hazard_cmd_ = create_publisher( "~/output/hazard", rclcpp::QoS{1}); pub_gear_cmd_ = - create_publisher("~/output/gear", rclcpp::QoS{1}); + create_publisher("~/output/gear", rclcpp::QoS{1}); pub_mrm_state_ = create_publisher("~/output/mrm/state", rclcpp::QoS{1}); @@ -75,9 +73,9 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) // Initialize odom_ = std::make_shared(); - control_mode_ = std::make_shared(); - prev_control_command_ = autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr( - new autoware_auto_control_msgs::msg::AckermannControlCommand); + 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(); @@ -93,19 +91,18 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) } void EmergencyHandler::onHazardStatusStamped( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg) + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg) { hazard_status_stamped_ = msg; stamp_hazard_status_ = this->now(); } void EmergencyHandler::onPrevControlCommand( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) + const autoware_control_msgs::msg::Control::ConstSharedPtr msg) { - auto control_command = new autoware_auto_control_msgs::msg::AckermannControlCommand(*msg); + auto control_command = new autoware_control_msgs::msg::Control(*msg); control_command->stamp = msg->stamp; - prev_control_command_ = - autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr(control_command); + prev_control_command_ = autoware_control_msgs::msg::Control::ConstSharedPtr(control_command); } void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) @@ -114,7 +111,7 @@ void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr } void EmergencyHandler::onControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; } @@ -131,9 +128,9 @@ void EmergencyHandler::onMrmEmergencyStopStatus( mrm_emergency_stop_status_ = msg; } -autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() +autoware_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() { - using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using autoware_vehicle_msgs::msg::HazardLightsCommand; HazardLightsCommand msg; // Check emergency @@ -155,7 +152,7 @@ autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHaz void EmergencyHandler::publishControlCommands() { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; // Create timestamp const auto stamp = this->now(); @@ -373,7 +370,7 @@ void EmergencyHandler::transitionTo(const int new_state) void EmergencyHandler::updateMrmState() { using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_auto_vehicle_msgs::msg::ControlModeReport; + using autoware_vehicle_msgs::msg::ControlModeReport; // Check emergency const bool is_emergency = isEmergency(); @@ -416,7 +413,7 @@ void EmergencyHandler::updateMrmState() autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurrentMrmBehavior() { using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; // Get hazard level auto level = hazard_status_stamped_->status.level; diff --git a/system/hazard_status_converter/package.xml b/system/hazard_status_converter/package.xml index 35ff28998ee80..addf51edbb8e9 100644 --- a/system/hazard_status_converter/package.xml +++ b/system/hazard_status_converter/package.xml @@ -11,7 +11,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_system_msgs + autoware_system_msgs diagnostic_graph_utils diagnostic_msgs rclcpp diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp index 9aa354c124bba..e8213b441bb33 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/system/hazard_status_converter/src/converter.cpp @@ -61,7 +61,7 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph) { using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; using DiagnosticLevel = DiagnosticStatus::_level_type; - using HazardStatus = autoware_auto_system_msgs::msg::HazardStatus; + using HazardStatus = autoware_system_msgs::msg::HazardStatus; using HazardLevel = HazardStatus::_level_type; const auto get_hazard_level = [](DiagnosticLevel unit_level, DiagnosticLevel root_level) { diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp index ad14ddde2bf5c..442eedf588429 100644 --- a/system/hazard_status_converter/src/converter.hpp +++ b/system/hazard_status_converter/src/converter.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -31,7 +31,7 @@ class Converter : public rclcpp::Node explicit Converter(const rclcpp::NodeOptions & options); private: - using HazardStatusStamped = autoware_auto_system_msgs::msg::HazardStatusStamped; + using HazardStatusStamped = autoware_system_msgs::msg::HazardStatusStamped; using DiagGraph = diagnostic_graph_utils::DiagGraph; using DiagUnit = diagnostic_graph_utils::DiagUnit; void on_create(DiagGraph::ConstSharedPtr graph); diff --git a/system/mrm_emergency_stop_operator/README.md b/system/mrm_emergency_stop_operator/README.md index 0866202f4352a..203303c0ea748 100644 --- a/system/mrm_emergency_stop_operator/README.md +++ b/system/mrm_emergency_stop_operator/README.md @@ -10,18 +10,18 @@ MRM emergency stop operator is a node that generates emergency stop commands acc ### Input -| Name | Type | Description | -| ------------------------------------ | ---------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------- | -| `~/input/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM execution order | -| `~/input/control/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Control command output from the last node of the control component. Used for the initial value of the emergency stop command. | -| | | | +| Name | Type | Description | +| ------------------------------------ | ------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------- | +| `~/input/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM execution order | +| `~/input/control/control_cmd` | `autoware_control_msgs::msg::Control` | Control command output from the last node of the control component. Used for the initial value of the emergency stop command. | +| | | | ### Output -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------------- | ---------------------- | -| `~/output/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | MRM execution status | -| `~/output/mrm/emergency_stop/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Emergency stop command | +| Name | Type | Description | +| ----------------------------------------- | ------------------------------------------- | ---------------------- | +| `~/output/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | MRM execution status | +| `~/output/mrm/emergency_stop/control_cmd` | `autoware_control_msgs::msg::Control` | Emergency stop command | ## Parameters diff --git a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp index 5b91f56973ae9..d7995a51ac8fb 100644 --- a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp +++ b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp @@ -20,7 +20,7 @@ #include // Autoware -#include +#include #include #include @@ -30,7 +30,7 @@ #include namespace mrm_emergency_stop_operator { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using tier4_system_msgs::msg::MrmBehaviorStatus; using tier4_system_msgs::srv::OperateMrm; @@ -55,9 +55,9 @@ class MrmEmergencyStopOperator : public rclcpp::Node const std::vector & parameters); // Subscriber - rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; - void onControlCommand(AckermannControlCommand::ConstSharedPtr msg); + void onControlCommand(Control::ConstSharedPtr msg); // Server rclcpp::Service::SharedPtr service_operation_; @@ -67,10 +67,10 @@ class MrmEmergencyStopOperator : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_status_; - rclcpp::Publisher::SharedPtr pub_control_cmd_; + rclcpp::Publisher::SharedPtr pub_control_cmd_; void publishStatus() const; - void publishControlCommand(const AckermannControlCommand & command) const; + void publishControlCommand(const Control & command) const; // Timer rclcpp::TimerBase::SharedPtr timer_; @@ -79,12 +79,11 @@ class MrmEmergencyStopOperator : public rclcpp::Node // States MrmBehaviorStatus status_; - AckermannControlCommand prev_control_cmd_; + Control prev_control_cmd_; bool is_prev_control_cmd_subscribed_; // Algorithm - AckermannControlCommand calcTargetAcceleration( - const AckermannControlCommand & prev_control_cmd) const; + Control calcTargetAcceleration(const Control & prev_control_cmd) const; }; } // namespace mrm_emergency_stop_operator diff --git a/system/mrm_emergency_stop_operator/package.xml b/system/mrm_emergency_stop_operator/package.xml index 6ca4e3a815f72..74ee8c7183741 100644 --- a/system/mrm_emergency_stop_operator/package.xml +++ b/system/mrm_emergency_stop_operator/package.xml @@ -12,7 +12,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs + autoware_control_msgs rclcpp rclcpp_components std_msgs diff --git a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp index 9ee9d7a685df3..589a2fd14d990 100644 --- a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +++ b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp @@ -28,7 +28,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n params_.target_jerk = declare_parameter("target_jerk", -1.5); // Subscriber - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control/control_cmd", 1, std::bind(&MrmEmergencyStopOperator::onControlCommand, this, std::placeholders::_1)); @@ -40,8 +40,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n // Publisher pub_status_ = create_publisher("~/output/mrm/emergency_stop/status", 1); - pub_control_cmd_ = - create_publisher("~/output/mrm/emergency_stop/control_cmd", 1); + pub_control_cmd_ = create_publisher("~/output/mrm/emergency_stop/control_cmd", 1); // Timer const auto update_period_ns = rclcpp::Rate(params_.update_rate).period(); @@ -70,7 +69,7 @@ rcl_interfaces::msg::SetParametersResult MrmEmergencyStopOperator::onParameter( return result; } -void MrmEmergencyStopOperator::onControlCommand(AckermannControlCommand::ConstSharedPtr msg) +void MrmEmergencyStopOperator::onControlCommand(Control::ConstSharedPtr msg) { if (status_.state != MrmBehaviorStatus::OPERATING) { prev_control_cmd_ = *msg; @@ -97,7 +96,7 @@ void MrmEmergencyStopOperator::publishStatus() const pub_status_->publish(status); } -void MrmEmergencyStopOperator::publishControlCommand(const AckermannControlCommand & command) const +void MrmEmergencyStopOperator::publishControlCommand(const Control & command) const { pub_control_cmd_->publish(command); } @@ -114,15 +113,14 @@ void MrmEmergencyStopOperator::onTimer() publishStatus(); } -AckermannControlCommand MrmEmergencyStopOperator::calcTargetAcceleration( - const AckermannControlCommand & prev_control_cmd) const +Control MrmEmergencyStopOperator::calcTargetAcceleration(const Control & prev_control_cmd) const { - auto control_cmd = AckermannControlCommand(); + auto control_cmd = Control(); if (!is_prev_control_cmd_subscribed_) { control_cmd.stamp = this->now(); control_cmd.longitudinal.stamp = this->now(); - control_cmd.longitudinal.speed = 0.0; + control_cmd.longitudinal.velocity = 0.0; control_cmd.longitudinal.acceleration = static_cast(params_.target_acceleration); control_cmd.longitudinal.jerk = 0.0; control_cmd.lateral.stamp = this->now(); @@ -136,8 +134,8 @@ AckermannControlCommand MrmEmergencyStopOperator::calcTargetAcceleration( control_cmd.stamp = this->now(); control_cmd.longitudinal.stamp = this->now(); - control_cmd.longitudinal.speed = static_cast(std::max( - prev_control_cmd.longitudinal.speed + prev_control_cmd.longitudinal.acceleration * dt, 0.0)); + control_cmd.longitudinal.velocity = static_cast(std::max( + prev_control_cmd.longitudinal.velocity + prev_control_cmd.longitudinal.acceleration * dt, 0.0)); control_cmd.longitudinal.acceleration = static_cast(std::max( prev_control_cmd.longitudinal.acceleration + params_.target_jerk * dt, params_.target_acceleration)); diff --git a/system/mrm_handler/README.md b/system/mrm_handler/README.md index 4944d8a833643..8ccb95e6ca8d3 100644 --- a/system/mrm_handler/README.md +++ b/system/mrm_handler/README.md @@ -14,26 +14,26 @@ MRM Handler is a node to select a proper MRM from a system failure state contain ### Input -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------------- | --------------------------------------------------------------------------------------------------- | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/system/operation_mode/availability` | `tier4_system_msgs::msg::OperationModeAvailability` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability | -| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | -| `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | -| `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | -| `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | -| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. | +| Name | Type | Description | +| -------------------------------------- | --------------------------------------------------- | --------------------------------------------------------------------------------------------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | +| `/system/operation_mode/availability` | `tier4_system_msgs::msg::OperationModeAvailability` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability | +| `/vehicle/status/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | +| `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | +| `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | +| `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | +| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. | ### Output -| Name | Type | Description | -| --------------------------------------- | ------------------------------------------------------ | ----------------------------------------------------- | -| `/system/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | -| `/system/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | -| `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | -| `/system/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | -| `/system/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | -| `/system/mrm/pull_over_manager/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM pull over | +| Name | Type | Description | +| --------------------------------------- | ------------------------------------------------- | ----------------------------------------------------- | +| `/system/emergency/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | +| `/system/emergency/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | +| `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | +| `/system/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | +| `/system/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | +| `/system/mrm/pull_over_manager/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM pull over | ## Parameters 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 c7aaca96aae49..8c58d961953ce 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -24,9 +24,9 @@ // Autoware #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -68,8 +68,7 @@ class MrmHandler : public rclcpp::Node // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; + rclcpp::Subscription::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; rclcpp::Subscription::SharedPtr @@ -82,7 +81,7 @@ class MrmHandler : public rclcpp::Node sub_operation_mode_state_; nav_msgs::msg::Odometry::ConstSharedPtr odom_; - autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; 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_; @@ -90,7 +89,7 @@ class MrmHandler : public rclcpp::Node autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_; void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::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); @@ -105,9 +104,8 @@ class MrmHandler : public rclcpp::Node // rclcpp::Publisher::SharedPtr pub_shift_; // rclcpp::Publisher::SharedPtr pub_turn_signal_; - rclcpp::Publisher::SharedPtr - pub_hazard_cmd_; - rclcpp::Publisher::SharedPtr pub_gear_cmd_; + rclcpp::Publisher::SharedPtr pub_hazard_cmd_; + rclcpp::Publisher::SharedPtr pub_gear_cmd_; void publishHazardCmd(); void publishGearCmd(); diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml index c15bc98a6c190..2db22cecaa82d 100644 --- a/system/mrm_handler/package.xml +++ b/system/mrm_handler/package.xml @@ -13,9 +13,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_system_msgs + autoware_vehicle_msgs nav_msgs rclcpp rclcpp_components 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 5e709946c2f46..afcc7ebd208ab 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -40,7 +40,7 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" sub_odom_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmHandler::onOdometry, this, _1)); // subscribe control mode - sub_control_mode_ = create_subscription( + sub_control_mode_ = create_subscription( "~/input/control_mode", rclcpp::QoS{1}, std::bind(&MrmHandler::onControlMode, this, _1)); sub_operation_mode_availability_ = create_subscription( @@ -60,10 +60,10 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" std::bind(&MrmHandler::onOperationModeState, this, _1)); // Publisher - pub_hazard_cmd_ = create_publisher( + pub_hazard_cmd_ = create_publisher( "~/output/hazard", rclcpp::QoS{1}); pub_gear_cmd_ = - create_publisher("~/output/gear", rclcpp::QoS{1}); + create_publisher("~/output/gear", rclcpp::QoS{1}); pub_mrm_state_ = create_publisher("~/output/mrm/state", rclcpp::QoS{1}); @@ -85,7 +85,7 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" // Initialize odom_ = std::make_shared(); - control_mode_ = std::make_shared(); + control_mode_ = std::make_shared(); mrm_pull_over_status_ = std::make_shared(); mrm_comfortable_stop_status_ = std::make_shared(); @@ -108,7 +108,7 @@ void MrmHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) } void MrmHandler::onControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; } @@ -167,7 +167,7 @@ void MrmHandler::onOperationModeState( void MrmHandler::publishHazardCmd() { - using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using autoware_vehicle_msgs::msg::HazardLightsCommand; HazardLightsCommand msg; msg.stamp = this->now(); @@ -187,7 +187,7 @@ void MrmHandler::publishHazardCmd() void MrmHandler::publishGearCmd() { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; GearCommand msg; msg.stamp = this->now(); @@ -444,7 +444,7 @@ void MrmHandler::transitionTo(const int new_state) void MrmHandler::updateMrmState() { using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_auto_vehicle_msgs::msg::ControlModeReport; + using autoware_vehicle_msgs::msg::ControlModeReport; // Check emergency const bool is_emergency = isEmergency(); diff --git a/system/system_diagnostic_monitor/config/map.yaml b/system/system_diagnostic_monitor/config/map.yaml index 45c1db294119f..231ac6eb5fa44 100644 --- a/system/system_diagnostic_monitor/config/map.yaml +++ b/system/system_diagnostic_monitor/config/map.yaml @@ -13,4 +13,4 @@ units: - path: /autoware/map/topic_rate_check/pointcloud_map type: diag node: topic_state_monitor_pointcloud_map - name: map_topic_status" + name: map_topic_status diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml index b00fcb8a26f68..8e2566a747abf 100644 --- a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml +++ b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml @@ -1,6 +1,8 @@ + + diff --git a/system/system_error_monitor/README.md b/system/system_error_monitor/README.md index a62d4b0f00576..e765239f30f4d 100644 --- a/system/system_error_monitor/README.md +++ b/system/system_error_monitor/README.md @@ -90,19 +90,19 @@ endif ### Input -| Name | Type | Description | -| ---------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | -| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](./config/diagnostic_aggregator) is used to | -| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | -| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | -| `/vehicle/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | +| Name | Type | Description | +| ---------------------------- | ----------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | +| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](./config/diagnostic_aggregator) is used to | +| `/autoware/state` | `autoware_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | +| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | +| `/vehicle/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | ### Output -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------------------------------ | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | HazardStatus contains system hazard level, emergency hold status and failure details | -| `/diagnostics_err` | `diagnostic_msgs::msg::DiagnosticArray` | This has the same contents as HazardStatus. This is used for visualization | +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ------------------------------------------------------------------------------------ | +| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | HazardStatus contains system hazard level, emergency hold status and failure details | +| `/diagnostics_err` | `diagnostic_msgs::msg::DiagnosticArray` | This has the same contents as HazardStatus. This is used for visualization | ## Parameters diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 8829bcc1edde2..22f1f4e9012c7 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -20,9 +20,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -86,7 +86,7 @@ class AutowareErrorMonitor : public rclcpp::Node rclcpp::Time emergency_state_switch_time_; rclcpp::Time initialized_time_; - autoware_auto_system_msgs::msg::HazardStatus hazard_status_{}; + autoware_system_msgs::msg::HazardStatus hazard_status_{}; std::unordered_map required_modules_map_; std::string current_mode_; @@ -101,28 +101,25 @@ class AutowareErrorMonitor : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_diag_array_; - rclcpp::Subscription::SharedPtr - sub_autoware_state_; + rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr sub_current_gate_mode_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; - void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg); + rclcpp::Subscription::SharedPtr sub_control_mode_; + void onAutowareState(const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg); void onCurrentGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg); - void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); + void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); void onDiagArray(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg); const size_t diag_buffer_size_ = 100; std::unordered_map diag_buffer_map_; diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diag_array_; - autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_; + autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_; tier4_control_msgs::msg::GateMode::ConstSharedPtr current_gate_mode_; - autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; // Publisher - rclcpp::Publisher::SharedPtr - pub_hazard_status_; + rclcpp::Publisher::SharedPtr pub_hazard_status_; rclcpp::Publisher::SharedPtr pub_diagnostics_err_; - void publishHazardStatus(const autoware_auto_system_msgs::msg::HazardStatus & hazard_status); + void publishHazardStatus(const autoware_system_msgs::msg::HazardStatus & hazard_status); // Service rclcpp::Service::SharedPtr srv_clear_emergency_; @@ -141,14 +138,14 @@ class AutowareErrorMonitor : public rclcpp::Node uint8_t getHazardLevel(const DiagConfig & required_module, const int diag_level) const; void appendHazardDiag( const DiagConfig & required_module, const diagnostic_msgs::msg::DiagnosticStatus & diag, - autoware_auto_system_msgs::msg::HazardStatus * hazard_status) const; - autoware_auto_system_msgs::msg::HazardStatus judgeHazardStatus() const; + autoware_system_msgs::msg::HazardStatus * hazard_status) const; + autoware_system_msgs::msg::HazardStatus judgeHazardStatus() const; void updateHazardStatus(); void updateTimeoutHazardStatus(); bool canAutoRecovery() const; bool isEmergencyHoldingRequired() const; - void loggingErrors(const autoware_auto_system_msgs::msg::HazardStatus & diag_array); + void loggingErrors(const autoware_system_msgs::msg::HazardStatus & diag_array); std::unique_ptr logger_configure_; }; diff --git a/system/system_error_monitor/package.xml b/system/system_error_monitor/package.xml index ed50eda95611a..0c8e2fc0241aa 100644 --- a/system/system_error_monitor/package.xml +++ b/system/system_error_monitor/package.xml @@ -10,8 +10,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_system_msgs + autoware_vehicle_msgs diagnostic_msgs fmt rclcpp diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index e4ca33204f53e..ad67521298643 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -96,9 +96,9 @@ bool isOverLevel(const int & diag_level, const std::string & failure_level_str) } std::vector & getTargetDiagnosticsRef( - const int hazard_level, autoware_auto_system_msgs::msg::HazardStatus * hazard_status) + const int hazard_level, autoware_system_msgs::msg::HazardStatus * hazard_status) { - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; if (hazard_level == HazardStatus::NO_FAULT) { return hazard_status->diag_no_fault; @@ -117,8 +117,7 @@ std::vector & getTargetDiagnosticsRef( } diagnostic_msgs::msg::DiagnosticArray convertHazardStatusToDiagnosticArray( - rclcpp::Clock::SharedPtr clock, - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) + rclcpp::Clock::SharedPtr clock, const autoware_system_msgs::msg::HazardStatus & hazard_status) { using diagnostic_msgs::msg::DiagnosticStatus; @@ -150,11 +149,10 @@ diagnostic_msgs::msg::DiagnosticArray convertHazardStatusToDiagnosticArray( } std::set getErrorModules( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status, - const int emergency_hazard_level) + const autoware_system_msgs::msg::HazardStatus & hazard_status, const int emergency_hazard_level) { std::set error_modules; - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; if (emergency_hazard_level <= HazardStatus::SINGLE_POINT_FAULT) { for (const auto & diag_spf : hazard_status.diag_single_point_fault) { error_modules.insert(diag_spf.name); @@ -175,10 +173,10 @@ std::set getErrorModules( } int isInNoFaultCondition( - const autoware_auto_system_msgs::msg::AutowareState & autoware_state, + const autoware_system_msgs::msg::AutowareState & autoware_state, const tier4_control_msgs::msg::GateMode & current_gate_mode) { - using autoware_auto_system_msgs::msg::AutowareState; + using autoware_system_msgs::msg::AutowareState; using tier4_control_msgs::msg::GateMode; const auto is_in_autonomous_ignore_state = @@ -221,7 +219,7 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) get_parameter_or("hazard_recovery_timeout", params_.hazard_recovery_timeout, 5.0); get_parameter_or( "emergency_hazard_level", params_.emergency_hazard_level, - autoware_auto_system_msgs::msg::HazardStatus::LATENT_FAULT); + autoware_system_msgs::msg::HazardStatus::LATENT_FAULT); get_parameter_or("use_emergency_hold", params_.use_emergency_hold, false); get_parameter_or( "use_emergency_hold_in_manual_driving", params_.use_emergency_hold_in_manual_driving, false); @@ -237,15 +235,15 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) sub_current_gate_mode_ = create_subscription( "~/input/current_gate_mode", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onCurrentGateMode, this, _1)); - sub_autoware_state_ = create_subscription( + sub_autoware_state_ = create_subscription( "~/input/autoware_state", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onAutowareState, this, _1)); - sub_control_mode_ = create_subscription( + sub_control_mode_ = create_subscription( "~/input/control_mode", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onControlMode, this, _1)); // Publisher - pub_hazard_status_ = create_publisher( + pub_hazard_status_ = create_publisher( "~/output/hazard_status", rclcpp::QoS{1}); pub_diagnostics_err_ = create_publisher( "~/output/diagnostics_err", rclcpp::QoS{1}); @@ -256,10 +254,10 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) std::bind(&AutowareErrorMonitor::onClearEmergencyService, this, _1, _2)); // Initialize - autoware_auto_vehicle_msgs::msg::ControlModeReport vehicle_state_report; - vehicle_state_report.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL; - control_mode_ = std::make_shared( - vehicle_state_report); + autoware_vehicle_msgs::msg::ControlModeReport vehicle_state_report; + vehicle_state_report.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL; + control_mode_ = + std::make_shared(vehicle_state_report); // Timer initialized_time_ = this->now(); @@ -363,7 +361,7 @@ void AutowareErrorMonitor::onCurrentGateMode( } void AutowareErrorMonitor::onAutowareState( - const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg) + const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg) { autoware_state_ = msg; @@ -372,7 +370,7 @@ void AutowareErrorMonitor::onAutowareState( } void AutowareErrorMonitor::onControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; @@ -486,7 +484,7 @@ boost::optional AutowareErrorMonitor::getLatestDiag( uint8_t AutowareErrorMonitor::getHazardLevel( const DiagConfig & required_module, const int diag_level) const { - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; if (isOverLevel(diag_level, required_module.spf_at)) { return HazardStatus::SINGLE_POINT_FAULT; @@ -503,7 +501,7 @@ uint8_t AutowareErrorMonitor::getHazardLevel( void AutowareErrorMonitor::appendHazardDiag( const DiagConfig & required_module, const diagnostic_msgs::msg::DiagnosticStatus & hazard_diag, - autoware_auto_system_msgs::msg::HazardStatus * hazard_status) const + autoware_system_msgs::msg::HazardStatus * hazard_status) const { const auto hazard_level = getHazardLevel(required_module, hazard_diag.level); @@ -520,12 +518,12 @@ void AutowareErrorMonitor::appendHazardDiag( hazard_status->level = std::max(hazard_status->level, hazard_level); } -autoware_auto_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardStatus() const +autoware_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardStatus() const { - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; using diagnostic_msgs::msg::DiagnosticStatus; - autoware_auto_system_msgs::msg::HazardStatus hazard_status; + autoware_system_msgs::msg::HazardStatus hazard_status; for (const auto & required_module : required_modules_map_.at(current_mode_)) { const auto & diag_name = required_module.name; const auto latest_diag = getLatestDiag(diag_name); @@ -566,7 +564,7 @@ autoware_auto_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardSt // Ignore error when vehicle is not ready to start if (isInNoFaultCondition(*autoware_state_, *current_gate_mode_)) { - hazard_status.level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT; + hazard_status.level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; } return hazard_status; @@ -604,7 +602,7 @@ void AutowareErrorMonitor::updateTimeoutHazardStatus() { const bool prev_emergency_status = hazard_status_.emergency; - hazard_status_.level = autoware_auto_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; + hazard_status_.level = autoware_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; hazard_status_.emergency = true; if (hazard_status_.emergency != prev_emergency_status) { @@ -663,7 +661,7 @@ bool AutowareErrorMonitor::isEmergencyHoldingRequired() const // Don't hold status during manual driving const bool is_manual_driving = - (control_mode_->mode == autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL); + (control_mode_->mode == autoware_vehicle_msgs::msg::ControlModeReport::MANUAL); const auto no_hold_condition = (!params_.use_emergency_hold_in_manual_driving && is_manual_driving); if (no_hold_condition) { @@ -674,9 +672,9 @@ bool AutowareErrorMonitor::isEmergencyHoldingRequired() const } void AutowareErrorMonitor::publishHazardStatus( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) + const autoware_system_msgs::msg::HazardStatus & hazard_status) { - autoware_auto_system_msgs::msg::HazardStatusStamped hazard_status_stamped; + autoware_system_msgs::msg::HazardStatusStamped hazard_status_stamped; hazard_status_stamped.stamp = this->now(); hazard_status_stamped.status = hazard_status; pub_hazard_status_->publish(hazard_status_stamped); @@ -700,7 +698,7 @@ bool AutowareErrorMonitor::onClearEmergencyService( } void AutowareErrorMonitor::loggingErrors( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) + const autoware_system_msgs::msg::HazardStatus & hazard_status) { if ( autoware_state_ && current_gate_mode_ && diff --git a/tools/reaction_analyzer/README.md b/tools/reaction_analyzer/README.md index f5a22aaf20176..ba77cc4c8f5bb 100644 --- a/tools/reaction_analyzer/README.md +++ b/tools/reaction_analyzer/README.md @@ -201,21 +201,21 @@ for each of the nodes. - `sensor_msgs/msg/Image` - `geometry_msgs/msg/PoseWithCovarianceStamped` - `sensor_msgs/msg/Imu` - - `autoware_auto_vehicle_msgs/msg/ControlModeReport` - - `autoware_auto_vehicle_msgs/msg/GearReport` - - `autoware_auto_vehicle_msgs/msg/HazardLightsReport` - - `autoware_auto_vehicle_msgs/msg/SteeringReport` - - `autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport` - - `autoware_auto_vehicle_msgs/msg/VelocityReport` + - `autoware_vehicle_msgs/msg/ControlModeReport` + - `autoware_vehicle_msgs/msg/GearReport` + - `autoware_vehicle_msgs/msg/HazardLightsReport` + - `autoware_vehicle_msgs/msg/SteeringReport` + - `autoware_vehicle_msgs/msg/TurnIndicatorsReport` + - `autoware_vehicle_msgs/msg/VelocityReport` - **Subscriber Message Types:** - `sensor_msgs/msg/PointCloud2` - - `autoware_auto_perception_msgs/msg/DetectedObjects` - - `autoware_auto_perception_msgs/msg/TrackedObjects` - - `autoware_auto_msgs/msg/PredictedObject` - - `autoware_auto_planning_msgs/msg/Trajectory` - - `autoware_auto_control_msgs/msg/AckermannControlCommand` + - `autoware_perception_msgs/msg/DetectedObjects` + - `autoware_perception_msgs/msg/TrackedObjects` + - `autoware_perception_msgs/msg/PredictedObject` + - `autoware_planning_msgs/msg/Trajectory` + - `autoware_control_msgs/msg/Control` - **Reaction Types:** - `FIRST_BRAKE` diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp index 6dc3dd896dc92..c5a8f6a2e663a 100644 --- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -42,9 +42,9 @@ using autoware_adapi_v1_msgs::msg::LocalizationInitializationState; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_adapi_v1_msgs::msg::RouteState; using autoware_adapi_v1_msgs::srv::ChangeOperationMode; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_internal_msgs::msg::PublishedTime; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; diff --git a/tools/reaction_analyzer/include/subscriber.hpp b/tools/reaction_analyzer/include/subscriber.hpp index d14d41da545f7..21d370a68b556 100644 --- a/tools/reaction_analyzer/include/subscriber.hpp +++ b/tools/reaction_analyzer/include/subscriber.hpp @@ -39,23 +39,23 @@ namespace reaction_analyzer::subscriber { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_control_msgs::msg::Control; using autoware_internal_msgs::msg::PublishedTime; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; // Buffers to be used to store subscribed messages' data, pipeline Header, and PublishedTime using MessageBuffer = std::optional; -// We need to store the past AckermannControlCommands to analyze the first brake -using ControlCommandBuffer = std::pair, MessageBuffer>; +// We need to store the past Control to analyze the first brake +using ControlCommandBuffer = std::pair, MessageBuffer>; // Variant to store different types of buffers using MessageBufferVariant = std::variant; @@ -67,7 +67,7 @@ struct SubscriberVariables std::unique_ptr> sub1_; std::unique_ptr> sub2_; std::unique_ptr> synchronizer_; - // tmp: only for the messages who don't have header e.g. AckermannControlCommand + // tmp: only for the messages who don't have header e.g. Control std::unique_ptr> cache_; }; @@ -75,7 +75,7 @@ struct SubscriberVariables using SubscriberVariablesVariant = std::variant< SubscriberVariables, SubscriberVariables, SubscriberVariables, SubscriberVariables, - SubscriberVariables, SubscriberVariables>; + SubscriberVariables, SubscriberVariables>; // The configuration of the topic to be subscribed which are defined in reaction_chain struct TopicConfig @@ -153,14 +153,11 @@ class SubscriberBase bool init_subscribers(); std::optional get_subscriber_variable( const TopicConfig & topic_config); - std::optional find_first_brake_idx( - const std::vector & cmd_array); - void set_control_command_to_buffer( - std::vector & buffer, const AckermannControlCommand & cmd) const; + std::optional find_first_brake_idx(const std::vector & cmd_array); + void set_control_command_to_buffer(std::vector & buffer, const Control & cmd) const; // Callbacks for modules are subscribed - void on_control_command( - const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr); + void on_control_command(const std::string & node_name, const Control::ConstSharedPtr & msg_ptr); void on_trajectory(const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr); void on_trajectory( const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr, diff --git a/tools/reaction_analyzer/include/topic_publisher.hpp b/tools/reaction_analyzer/include/topic_publisher.hpp index c6d3a90650884..0c01561fec508 100644 --- a/tools/reaction_analyzer/include/topic_publisher.hpp +++ b/tools/reaction_analyzer/include/topic_publisher.hpp @@ -20,12 +20,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -45,12 +45,12 @@ namespace reaction_analyzer::topic_publisher { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_control_msgs::msg::Control; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; @@ -164,12 +164,12 @@ using PublisherVariablesVariant = std::variant< PublisherVariables, PublisherVariables, PublisherVariables, PublisherVariables, - PublisherVariables, - PublisherVariables, - PublisherVariables, - PublisherVariables, - PublisherVariables, - PublisherVariables>; + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables>; using LidarOutputPair = std::pair< std::shared_ptr>, diff --git a/tools/reaction_analyzer/include/utils.hpp b/tools/reaction_analyzer/include/utils.hpp index da1defc03f34c..4a31da363deff 100644 --- a/tools/reaction_analyzer/include/utils.hpp +++ b/tools/reaction_analyzer/include/utils.hpp @@ -22,12 +22,12 @@ #include #include -#include -#include -#include -#include -#include +#include #include +#include +#include +#include +#include #include #include #include @@ -52,16 +52,16 @@ // The reaction_analyzer namespace contains utility functions for the Reaction Analyzer tool. namespace reaction_analyzer { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_control_msgs::msg::Control; using autoware_internal_msgs::msg::PublishedTime; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using sensor_msgs::msg::PointCloud2; /** @@ -111,7 +111,7 @@ enum class PublisherMessageType { */ enum class SubscriberMessageType { UNKNOWN = 0, - ACKERMANN_CONTROL_COMMAND = 1, + CONTROL = 1, TRAJECTORY = 2, POINTCLOUD2 = 3, DETECTED_OBJECTS = 4, diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml index 05081eac01751..a5199aa78c9f8 100644 --- a/tools/reaction_analyzer/package.xml +++ b/tools/reaction_analyzer/package.xml @@ -15,12 +15,12 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs autoware_internal_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs eigen libpcl-all-dev message_filters diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml index 62c1191bd345a..b40a67ecc333c 100644 --- a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml +++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml @@ -54,27 +54,27 @@ obstacle_cruise_planner: topic_name: /planning/scenario_planning/lane_driving/trajectory time_debug_topic_name: /planning/scenario_planning/lane_driving/trajectory/debug/published_time - message_type: autoware_auto_planning_msgs/msg/Trajectory + message_type: autoware_planning_msgs/msg/Trajectory scenario_selector: topic_name: /planning/scenario_planning/scenario_selector/trajectory time_debug_topic_name: /planning/scenario_planning/scenario_selector/trajectory/debug/published_time - message_type: autoware_auto_planning_msgs/msg/Trajectory + message_type: autoware_planning_msgs/msg/Trajectory motion_velocity_smoother: topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory time_debug_topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory/debug/published_time - message_type: autoware_auto_planning_msgs/msg/Trajectory + message_type: autoware_planning_msgs/msg/Trajectory planning_validator: topic_name: /planning/scenario_planning/trajectory time_debug_topic_name: /planning/scenario_planning/trajectory/debug/published_time - message_type: autoware_auto_planning_msgs/msg/Trajectory + message_type: autoware_planning_msgs/msg/Trajectory trajectory_follower: topic_name: /control/trajectory_follower/control_cmd time_debug_topic_name: /control/trajectory_follower/control_cmd/debug/published_time - message_type: autoware_auto_control_msgs/msg/AckermannControlCommand + message_type: autoware_control_msgs/msg/Control vehicle_cmd_gate: topic_name: /control/command/control_cmd time_debug_topic_name: /control/command/control_cmd/debug/published_time - message_type: autoware_auto_control_msgs/msg/AckermannControlCommand + message_type: autoware_control_msgs/msg/Control common_ground_filter: topic_name: /perception/obstacle_segmentation/single_frame/pointcloud time_debug_topic_name: /perception/obstacle_segmentation/single_frame/pointcloud/debug/published_time @@ -86,32 +86,32 @@ multi_object_tracker: topic_name: /perception/object_recognition/tracking/near_objects time_debug_topic_name: /perception/object_recognition/tracking/near_objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/TrackedObjects + message_type: autoware_perception_msgs/msg/TrackedObjects lidar_centerpoint: topic_name: /perception/object_recognition/detection/centerpoint/objects time_debug_topic_name: /perception/object_recognition/detection/centerpoint/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/DetectedObjects + message_type: autoware_perception_msgs/msg/DetectedObjects obstacle_pointcloud_based_validator: topic_name: /perception/object_recognition/detection/centerpoint/validation/objects time_debug_topic_name: /perception/object_recognition/detection/centerpoint/validation/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/DetectedObjects + message_type: autoware_perception_msgs/msg/DetectedObjects decorative_tracker_merger: topic_name: /perception/object_recognition/tracking/objects time_debug_topic_name: /perception/object_recognition/tracking/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/TrackedObjects + message_type: autoware_perception_msgs/msg/TrackedObjects detected_object_feature_remover: topic_name: /perception/object_recognition/detection/clustering/objects time_debug_topic_name: /perception/object_recognition/detection/clustering/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/DetectedObjects + message_type: autoware_perception_msgs/msg/DetectedObjects detection_by_tracker: topic_name: /perception/object_recognition/detection/detection_by_tracker/objects time_debug_topic_name: /perception/object_recognition/detection/detection_by_tracker/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/DetectedObjects + message_type: autoware_perception_msgs/msg/DetectedObjects object_lanelet_filter: topic_name: /perception/object_recognition/detection/objects time_debug_topic_name: /perception/object_recognition/detection/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/DetectedObjects + message_type: autoware_perception_msgs/msg/DetectedObjects map_based_prediction: topic_name: /perception/object_recognition/objects time_debug_topic_name: /perception/object_recognition/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/PredictedObjects + message_type: autoware_perception_msgs/msg/PredictedObjects diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp index 8c735c42b9cd5..9064f42377d10 100644 --- a/tools/reaction_analyzer/src/subscriber.cpp +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -189,12 +189,12 @@ void SubscriberBase::reset() // Callbacks void SubscriberBase::on_control_command( - const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr) + const std::string & node_name, const Control::ConstSharedPtr & msg_ptr) { std::lock_guard lock(mutex_); auto & variant = message_buffers_[node_name]; if (!std::holds_alternative(variant)) { - ControlCommandBuffer buffer(std::vector{*msg_ptr}, std::nullopt); + ControlCommandBuffer buffer(std::vector{*msg_ptr}, std::nullopt); variant = buffer; } auto & cmd_buffer = std::get(variant); @@ -212,7 +212,7 @@ void SubscriberBase::on_control_command( // TODO(brkay54): update here if message_filters package add support for the messages which // does not have header const auto & subscriber_variant = - std::get>(subscriber_variables_map_[node_name]); + std::get>(subscriber_variables_map_[node_name]); // check if the cache was initialized or not, if there is, use it to set the published time if (subscriber_variant.cache_) { @@ -636,24 +636,23 @@ std::optional SubscriberBase::get_subscriber_variabl const TopicConfig & topic_config) { switch (topic_config.message_type) { - case SubscriberMessageType::ACKERMANN_CONTROL_COMMAND: { - SubscriberVariables subscriber_variable; + case SubscriberMessageType::CONTROL: { + SubscriberVariables subscriber_variable; if (!topic_config.time_debug_topic_address.empty()) { // If not empty, user should define a time debug topic // NOTE: Because message_filters package does not support the messages without headers, we // can not use the synchronizer. After we reacted, we are going to use the cache - // of the both PublishedTime and AckermannControlCommand subscribers to find the messages + // of the both PublishedTime and Control subscribers to find the messages // which have same header time. - std::function callback = - [this, topic_config](const AckermannControlCommand::ConstSharedPtr & ptr) { + std::function callback = + [this, topic_config](const Control::ConstSharedPtr & ptr) { this->on_control_command(topic_config.node_name, ptr); }; - subscriber_variable.sub1_ = - std::make_unique>( - node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), - create_subscription_options(node_)); + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); @@ -665,15 +664,14 @@ std::optional SubscriberBase::get_subscriber_variabl *subscriber_variable.sub2_, cache_size); } else { - std::function callback = - [this, topic_config](const AckermannControlCommand::ConstSharedPtr & ptr) { + std::function callback = + [this, topic_config](const Control::ConstSharedPtr & ptr) { this->on_control_command(topic_config.node_name, ptr); }; - subscriber_variable.sub1_ = - std::make_unique>( - node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), - create_subscription_options(node_)); + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); RCLCPP_WARN( @@ -915,8 +913,7 @@ std::optional SubscriberBase::get_subscriber_variabl } } -std::optional SubscriberBase::find_first_brake_idx( - const std::vector & cmd_array) +std::optional SubscriberBase::find_first_brake_idx(const std::vector & cmd_array) { if ( cmd_array.size() < @@ -975,7 +972,7 @@ std::optional SubscriberBase::find_first_brake_idx( } void SubscriberBase::set_control_command_to_buffer( - std::vector & buffer, const AckermannControlCommand & cmd) const + std::vector & buffer, const Control & cmd) const { const auto last_cmd_time = cmd.stamp; if (!buffer.empty()) { diff --git a/tools/reaction_analyzer/src/topic_publisher.cpp b/tools/reaction_analyzer/src/topic_publisher.cpp index 8a66bf9e33911..f720786d422bc 100644 --- a/tools/reaction_analyzer/src/topic_publisher.cpp +++ b/tools/reaction_analyzer/src/topic_publisher.cpp @@ -512,22 +512,22 @@ void TopicPublisher::init_rosbag_publisher_buffer( } else if (message_type == PublisherMessageType::IMU) { set_message(current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::CONTROL_MODE_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::GEAR_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::HAZARD_LIGHTS_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::STEERING_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::TURN_INDICATORS_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::VELOCITY_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } } diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp index 4a6110322440e..9f9198af7dc48 100644 --- a/tools/reaction_analyzer/src/utils.cpp +++ b/tools/reaction_analyzer/src/utils.cpp @@ -18,22 +18,22 @@ namespace reaction_analyzer { SubscriberMessageType get_subscriber_message_type(const std::string & message_type) { - if (message_type == "autoware_auto_control_msgs/msg/AckermannControlCommand") { - return SubscriberMessageType::ACKERMANN_CONTROL_COMMAND; + if (message_type == "autoware_control_msgs/msg/Control") { + return SubscriberMessageType::CONTROL; } - if (message_type == "autoware_auto_planning_msgs/msg/Trajectory") { + if (message_type == "autoware_planning_msgs/msg/Trajectory") { return SubscriberMessageType::TRAJECTORY; } if (message_type == "sensor_msgs/msg/PointCloud2") { return SubscriberMessageType::POINTCLOUD2; } - if (message_type == "autoware_auto_perception_msgs/msg/PredictedObjects") { + if (message_type == "autoware_perception_msgs/msg/PredictedObjects") { return SubscriberMessageType::PREDICTED_OBJECTS; } - if (message_type == "autoware_auto_perception_msgs/msg/DetectedObjects") { + if (message_type == "autoware_perception_msgs/msg/DetectedObjects") { return SubscriberMessageType::DETECTED_OBJECTS; } - if (message_type == "autoware_auto_perception_msgs/msg/TrackedObjects") { + if (message_type == "autoware_perception_msgs/msg/TrackedObjects") { return SubscriberMessageType::TRACKED_OBJECTS; } return SubscriberMessageType::UNKNOWN; @@ -62,22 +62,22 @@ PublisherMessageType get_publisher_message_type(const std::string & message_type if (message_type == "sensor_msgs/msg/Imu") { return PublisherMessageType::IMU; } - if (message_type == "autoware_auto_vehicle_msgs/msg/ControlModeReport") { + if (message_type == "autoware_vehicle_msgs/msg/ControlModeReport") { return PublisherMessageType::CONTROL_MODE_REPORT; } - if (message_type == "autoware_auto_vehicle_msgs/msg/GearReport") { + if (message_type == "autoware_vehicle_msgs/msg/GearReport") { return PublisherMessageType::GEAR_REPORT; } - if (message_type == "autoware_auto_vehicle_msgs/msg/HazardLightsReport") { + if (message_type == "autoware_vehicle_msgs/msg/HazardLightsReport") { return PublisherMessageType::HAZARD_LIGHTS_REPORT; } - if (message_type == "autoware_auto_vehicle_msgs/msg/SteeringReport") { + if (message_type == "autoware_vehicle_msgs/msg/SteeringReport") { return PublisherMessageType::STEERING_REPORT; } - if (message_type == "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport") { + if (message_type == "autoware_vehicle_msgs/msg/TurnIndicatorsReport") { return PublisherMessageType::TURN_INDICATORS_REPORT; } - if (message_type == "autoware_auto_vehicle_msgs/msg/VelocityReport") { + if (message_type == "autoware_vehicle_msgs/msg/VelocityReport") { return PublisherMessageType::VELOCITY_REPORT; } return PublisherMessageType::UNKNOWN; @@ -360,17 +360,17 @@ PredictedObjects::SharedPtr create_entity_predicted_objects_ptr(const EntityPara dimension.set__z(entity_params.z_l); obj.shape.set__dimensions(dimension); - obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.existence_probability = 1.0; obj.kinematics.initial_pose_with_covariance.pose = entity_pose; - autoware_auto_perception_msgs::msg::PredictedPath path; + autoware_perception_msgs::msg::PredictedPath path; path.confidence = 1.0; path.path.emplace_back(entity_pose); obj.kinematics.predicted_paths.emplace_back(path); - autoware_auto_perception_msgs::msg::ObjectClassification classification; - classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + autoware_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_perception_msgs::msg::ObjectClassification::CAR; classification.probability = 1.0; obj.classification.emplace_back(classification); obj.set__object_id(generate_uuid_msg("test_obstacle")); diff --git a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 3580f0546bbfb..6b94b7301540c 100644 --- a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -27,8 +27,8 @@ #include -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_msgs/msg/bool.hpp" @@ -56,8 +56,8 @@ namespace accel_brake_map_calibrator { -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::VelocityReport; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::VelocityReport; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::OccupancyGrid; using raw_vehicle_cmd_converter::AccelMap; diff --git a/vehicle/accel_brake_map_calibrator/package.xml b/vehicle/accel_brake_map_calibrator/package.xml index 64e7d1fb105cf..4c4eadea5e1ce 100644 --- a/vehicle/accel_brake_map_calibrator/package.xml +++ b/vehicle/accel_brake_map_calibrator/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs motion_utils diff --git a/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py b/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py index 749c9e429bf06..3a029e193f41f 100755 --- a/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py +++ b/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py @@ -15,7 +15,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_vehicle_msgs.msg import GearCommand +from autoware_vehicle_msgs.msg import GearCommand import rclpy from rclpy.node import Node from tier4_debug_msgs.msg import Float32Stamped diff --git a/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py b/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py index 9e57813bd4010..cd2205f8b269f 100755 --- a/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py +++ b/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py @@ -15,7 +15,7 @@ # limitations under the License. -from autoware_auto_vehicle_msgs.msg import VelocityReport +from autoware_vehicle_msgs.msg import VelocityReport import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/vehicle/external_cmd_converter/README.md b/vehicle/external_cmd_converter/README.md index 3c4181a6f093b..ef5865debd4d4 100644 --- a/vehicle/external_cmd_converter/README.md +++ b/vehicle/external_cmd_converter/README.md @@ -7,16 +7,16 @@ | Name | Type | Description | | --------------------------- | -------------------------------------------- | ----------------------------------------------------------------------------------------------------------------- | | `~/in/external_control_cmd` | tier4_external_api_msgs::msg::ControlCommand | target `throttle/brake/steering_angle/steering_angle_velocity` is necessary to calculate desired control command. | -| `~/input/shift_cmd"` | autoware_auto_vehicle_msgs::GearCommand | current command of gear. | +| `~/input/shift_cmd"` | autoware_vehicle_msgs::GearCommand | current command of gear. | | `~/input/emergency_stop` | tier4_external_api_msgs::msg::Heartbeat | emergency heart beat for external command. | | `~/input/current_gate_mode` | tier4_control_msgs::msg::GateMode | topic for gate mode. | | `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. | ## Output topics -| Name | Type | Description | -| ------------------- | -------------------------------------------------------- | ------------------------------------------------------------------ | -| `~/out/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | ackermann control command converted from selected external command | +| Name | Type | Description | +| ------------------- | ----------------------------------- | ------------------------------------------------------------------ | +| `~/out/control_cmd` | autoware_control_msgs::msg::Control | ackermann control command converted from selected external command | ## Parameters diff --git a/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp b/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp index f5a52ad4a6032..2488c8ea7f300 100644 --- a/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp +++ b/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -32,13 +32,12 @@ namespace external_cmd_converter { -using GearCommand = autoware_auto_vehicle_msgs::msg::GearCommand; -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; +using GearCommand = autoware_vehicle_msgs::msg::GearCommand; +using Control = autoware_control_msgs::msg::Control; using ExternalControlCommand = tier4_external_api_msgs::msg::ControlCommandStamped; using Odometry = nav_msgs::msg::Odometry; using raw_vehicle_cmd_converter::AccelMap; using raw_vehicle_cmd_converter::BrakeMap; -using ControlCommandStamped = autoware_auto_control_msgs::msg::AckermannControlCommand; using Odometry = nav_msgs::msg::Odometry; class ExternalCmdConverterNode : public rclcpp::Node @@ -48,7 +47,7 @@ class ExternalCmdConverterNode : public rclcpp::Node private: // Publisher - rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::Publisher::SharedPtr pub_current_cmd_; diff --git a/vehicle/external_cmd_converter/package.xml b/vehicle/external_cmd_converter/package.xml index d9301e23e05fe..513d73d2d39ff 100644 --- a/vehicle/external_cmd_converter/package.xml +++ b/vehicle/external_cmd_converter/package.xml @@ -12,8 +12,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs nav_msgs diff --git a/vehicle/external_cmd_converter/src/node.cpp b/vehicle/external_cmd_converter/src/node.cpp index 32592c02c3347..21945bb9078b8 100644 --- a/vehicle/external_cmd_converter/src/node.cpp +++ b/vehicle/external_cmd_converter/src/node.cpp @@ -26,7 +26,7 @@ ExternalCmdConverterNode::ExternalCmdConverterNode(const rclcpp::NodeOptions & n { using std::placeholders::_1; - pub_cmd_ = create_publisher("out/control_cmd", rclcpp::QoS{1}); + pub_cmd_ = create_publisher("out/control_cmd", rclcpp::QoS{1}); pub_current_cmd_ = create_publisher("out/latest_external_control_cmd", rclcpp::QoS{1}); sub_velocity_ = create_subscription( @@ -142,11 +142,11 @@ void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::Const } // Publish ControlCommand - autoware_auto_control_msgs::msg::AckermannControlCommand output; + autoware_control_msgs::msg::Control output; output.stamp = cmd_ptr->stamp; output.lateral.steering_tire_angle = cmd_ptr->control.steering_angle; output.lateral.steering_tire_rotation_rate = cmd_ptr->control.steering_angle_velocity; - output.longitudinal.speed = ref_velocity; + output.longitudinal.velocity = ref_velocity; output.longitudinal.acceleration = ref_acceleration; pub_cmd_->publish(output); diff --git a/vehicle/raw_vehicle_cmd_converter/README.md b/vehicle/raw_vehicle_cmd_converter/README.md index 5767c4fbf672b..6ac1ee4666cae 100644 --- a/vehicle/raw_vehicle_cmd_converter/README.md +++ b/vehicle/raw_vehicle_cmd_converter/README.md @@ -26,11 +26,11 @@ For ease of calibration and adjustments to the lookup table, an auto-calibration ## Input topics -| Name | Type | Description | -| --------------------- | -------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | -| `~/input/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | -| `~/input/steering"` | autoware_auto_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control | -| `~/input/twist` | navigation_msgs::Odometry | twist topic in odometry is used. | +| Name | Type | Description | +| --------------------- | ------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | +| `~/input/control_cmd` | autoware_control_msgs::msg::Control | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | +| `~/input/steering"` | autoware_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control | +| `~/input/twist` | navigation_msgs::Odometry | twist topic in odometry is used. | ## Output topics diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp index 98693337b5c28..d71ff96abade0 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp @@ -23,8 +23,8 @@ #include -#include -#include +#include +#include #include #include #include @@ -36,12 +36,12 @@ namespace raw_vehicle_cmd_converter { -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; +using Control = autoware_control_msgs::msg::Control; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_vehicle_msgs::msg::ActuationCommandStamped; using TwistStamped = geometry_msgs::msg::TwistStamped; using Odometry = nav_msgs::msg::Odometry; -using Steering = autoware_auto_vehicle_msgs::msg::SteeringReport; +using Steering = autoware_vehicle_msgs::msg::SteeringReport; class DebugValues { @@ -77,7 +77,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node //!< @brief subscriber for current velocity rclcpp::Subscription::SharedPtr sub_velocity_; //!< @brief subscriber for vehicle command - rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; //!< @brief subscriber for steering rclcpp::Subscription::SharedPtr sub_steering_; @@ -85,7 +85,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node std::unique_ptr current_twist_ptr_; // [m/s] std::unique_ptr current_steer_ptr_; - AckermannControlCommand::ConstSharedPtr control_cmd_ptr_; + Control::ConstSharedPtr control_cmd_ptr_; AccelMap accel_map_; BrakeMap brake_map_; SteerMap steer_map_; @@ -110,7 +110,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node double calculateBrakeMap(const double current_velocity, const double desired_acc); double calculateSteer(const double vel, const double steering, const double steer_rate); void onSteering(const Steering::ConstSharedPtr msg); - void onControlCmd(const AckermannControlCommand::ConstSharedPtr msg); + void onControlCmd(const Control::ConstSharedPtr msg); void onVelocity(const Odometry::ConstSharedPtr msg); void publishActuationCmd(); // for debugging diff --git a/vehicle/raw_vehicle_cmd_converter/package.xml b/vehicle/raw_vehicle_cmd_converter/package.xml index d68f4601aab67..376a5c74f1cb6 100644 --- a/vehicle/raw_vehicle_cmd_converter/package.xml +++ b/vehicle/raw_vehicle_cmd_converter/package.xml @@ -17,8 +17,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs geometry_msgs interpolation nav_msgs diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index 2506fb1f70b76..91c668f63dfbd 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -76,7 +76,7 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( steer_pid_.setInitialized(); } pub_actuation_cmd_ = create_publisher("~/output/actuation_cmd", 1); - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control_cmd", 1, std::bind(&RawVehicleCommandConverterNode::onControlCmd, this, _1)); sub_velocity_ = create_subscription( "~/input/odometry", 1, std::bind(&RawVehicleCommandConverterNode::onVelocity, this, _1)); @@ -216,7 +216,7 @@ void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr m current_twist_ptr_->twist = msg->twist.twist; } -void RawVehicleCommandConverterNode::onControlCmd(const AckermannControlCommand::ConstSharedPtr msg) +void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg) { control_cmd_ptr_ = msg; publishActuationCmd(); diff --git a/vehicle/steer_offset_estimator/Readme.md b/vehicle/steer_offset_estimator/Readme.md index 3b455a97e4925..164d411e31050 100644 --- a/vehicle/steer_offset_estimator/Readme.md +++ b/vehicle/steer_offset_estimator/Readme.md @@ -17,10 +17,10 @@ Calculate yaw rate error and then calculate steering error recursively by least ### Input -| Name | Type | Description | -| --------------- | ------------------------------------------------- | ------------- | -| `~/input/twist` | `geometry_msgs::msg::TwistStamped` | vehicle twist | -| `~/input/steer` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | steering | +| Name | Type | Description | +| --------------- | -------------------------------------------- | ------------- | +| `~/input/twist` | `geometry_msgs::msg::TwistStamped` | vehicle twist | +| `~/input/steer` | `autoware_vehicle_msgs::msg::SteeringReport` | steering | ### Output diff --git a/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp b/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp index 64d6694d7ce3e..afd1d0520e951 100644 --- a/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp +++ b/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp @@ -18,7 +18,7 @@ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" @@ -28,7 +28,7 @@ namespace steer_offset_estimator { using geometry_msgs::msg::TwistStamped; using tier4_debug_msgs::msg::Float32Stamped; -using Steering = autoware_auto_vehicle_msgs::msg::SteeringReport; +using Steering = autoware_vehicle_msgs::msg::SteeringReport; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; diff --git a/vehicle/steer_offset_estimator/package.xml b/vehicle/steer_offset_estimator/package.xml index 242ec5f2f148a..636d901d71ef5 100644 --- a/vehicle/steer_offset_estimator/package.xml +++ b/vehicle/steer_offset_estimator/package.xml @@ -9,7 +9,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs rclcpp