diff --git a/.clang-format b/.clang-format index b41fae9129e43..cd54eb45dde50 100644 --- a/.clang-format +++ b/.clang-format @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + # Modified from https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format Language: Cpp BasedOnStyle: Google diff --git a/.clang-tidy-ignore b/.clang-tidy-ignore new file mode 100644 index 0000000000000..f10528128dce6 --- /dev/null +++ b/.clang-tidy-ignore @@ -0,0 +1 @@ +*/examples/* diff --git a/.cspell.json b/.cspell.json index 2d024a5ca589d..f3bf484f68e0c 100644 --- a/.cspell.json +++ b/.cspell.json @@ -4,5 +4,5 @@ "planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/**" ], "ignoreRegExpList": [], - "words": ["dltype", "tvmgen", "fromarray", "soblin"] + "words": ["dltype", "tvmgen", "fromarray", "soblin", "brkay54", "libtensorrt"] } diff --git a/.github/ISSUE_TEMPLATE/bug.yaml b/.github/ISSUE_TEMPLATE/bug.yaml index 12a857998a0b2..5c74f7c5e4d9c 100644 --- a/.github/ISSUE_TEMPLATE/bug.yaml +++ b/.github/ISSUE_TEMPLATE/bug.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: Bug description: Report a bug body: diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 48765c24a7b25..deccbf336f6a3 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + blank_issues_enabled: false contact_links: - name: Question diff --git a/.github/ISSUE_TEMPLATE/task.yaml b/.github/ISSUE_TEMPLATE/task.yaml index cd8322f507405..58307325ce402 100644 --- a/.github/ISSUE_TEMPLATE/task.yaml +++ b/.github/ISSUE_TEMPLATE/task.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: Task description: Plan a task body: diff --git a/.github/CODEOWNERS b/.github/_CODEOWNERS similarity index 94% rename from .github/CODEOWNERS rename to .github/_CODEOWNERS index df6be468e726f..3c1defb31a741 100644 --- a/.github/CODEOWNERS +++ b/.github/_CODEOWNERS @@ -6,6 +6,8 @@ common/autoware_component_interface_tools/** isamu.takagi@tier4.jp common/autoware_component_interface_utils/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/autoware_fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_geography_utils/** anh.nguyen.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +common/autoware_global_parameter_loader/** ryohsuke.mitsudome@tier4.jp +common/autoware_glog_component/** takamasa.horibe@tier4.jp common/autoware_goal_distance_calculator/** taiki.tanaka@tier4.jp common/autoware_grid_map_utils/** maxime.clement@tier4.jp common/autoware_interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp @@ -24,13 +26,12 @@ common/autoware_signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.j common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -common/autoware_trajectory/** mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yukinari.hisaki.2@tier4.jp +common/autoware_traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/autoware_traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp +common/autoware_trajectory/** mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yukinari.hisaki.2@tier4.jp common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/autoware_vehicle_info_utils/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp -common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp -common/glog_component/** takamasa.horibe@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp @@ -41,17 +42,16 @@ common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp khal common/tier4_system_rviz_plugin/** koji.minoda@tier4.jp common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp -common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/autoware_collision_detector/** go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp tomohito.ando@tier4.jp control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp zulfaqar.azmi@tier4.jp -control/autoware_mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_mpc_lateral_controller/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_operation_mode_transition_manager/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -control/autoware_pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp control/autoware_pure_pursuit/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_shift_decider/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_smart_mpc_trajectory_follower/** kosuke.takeuchi@tier4.jp masayuki.aino@proxima-ai-tech.com takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -63,7 +63,7 @@ control/predicted_path_checker/** berkay@leodrive.ai evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp temkei.kem@tier4.jp evaluator/autoware_planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp temkei.kem@tier4.jp evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp +evaluator/localization_evaluator/** anh.nguyen.2@tier4.jp dominik.jargot@robotec.ai koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp evaluator/scenario_simulator_v2_adapter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp @@ -97,14 +97,14 @@ localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuu localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/autoware_lanelet2_map_visualizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/autoware_lanelet2_map_visualizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp mamoru.sobue@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/autoware_map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/autoware_map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/autoware_map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/autoware_map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp perception/autoware_bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp perception/autoware_cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -perception/autoware_compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_crosswalk_traffic_light_estimator/** masato.saeki@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/autoware_detected_object_feature_remover/** kotaro.uetake@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp perception/autoware_detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -169,12 +169,12 @@ planning/behavior_path_planner/autoware_behavior_path_external_request_lane_chan planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_planner/autoware_behavior_path_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/autoware_behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_planner/autoware_behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_planner_common/** alqudah.mohammad@tier4.jp daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp planning/behavior_path_planner/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_planner/autoware_behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yukinari.hisaki.2@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yukinari.hisaki.2@tier4.jp @@ -185,7 +185,7 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** kosuke. planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yukinari.hisaki.2@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -211,7 +211,7 @@ sensing/autoware_radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shuns sensing/autoware_radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/autoware_radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp -sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp +sensing/autoware_vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/autoware_carla_interface/** maxime.clement@tier4.jp mradityagio@gmail.com simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp @@ -245,5 +245,6 @@ vehicle/autoware_accel_brake_map_calibrator/** eiki.nagata.2@tier4.jp taiki.tana vehicle/autoware_external_cmd_converter/** eiki.nagata.2@tier4.jp takamasa.horibe@tier4.jp vehicle/autoware_raw_vehicle_cmd_converter/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp sho.iwasawa.2@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp +visualization/autoware_pyplot/** mamoru.sobue@tier4.jp yukinari.hisaki.2@tier4.jp ### Copied from .github/CODEOWNERS-manual ### diff --git a/.github/dependabot.yaml b/.github/dependabot.yaml index 0264c035357bd..8fd9b7f4ae0a5 100644 --- a/.github/dependabot.yaml +++ b/.github/dependabot.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + version: 2 updates: - package-ecosystem: github-actions diff --git a/.github/stale.yml b/.github/stale.yml index bc99e4383cafd..ffce036c8d047 100644 --- a/.github/stale.yml +++ b/.github/stale.yml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + # Modified from https://github.com/probot/stale#usage # Number of days of inactivity before an Issue or Pull Request with the stale label is closed diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index e70d3daf88509..4c09e072766fa 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -1,55 +1,48 @@ -- repository: autowarefoundation/autoware +- repository: autowarefoundation/sync-file-templates + source-dir: sources files: - - source: CODE_OF_CONDUCT.md - - source: CONTRIBUTING.md - - source: DISCLAIMER.md - - source: LICENSE - source: .github/ISSUE_TEMPLATE/bug.yaml - source: .github/ISSUE_TEMPLATE/config.yml - source: .github/ISSUE_TEMPLATE/task.yaml - source: .github/dependabot.yaml + - source: .github/pull_request_template_complex.md + dest: .github/pull_request_template.md - source: .github/stale.yml - source: .github/workflows/cancel-previous-workflows.yaml + - source: .github/workflows/check-build-depends.yaml + - source: .github/workflows/clang-tidy-pr-comments.yaml + - source: .github/workflows/clang-tidy-pr-comments-manually.yaml + - source: .github/workflows/comment-on-pr.yaml + - source: .github/workflows/delete-closed-pr-docs.yaml + - source: .github/workflows/deploy-docs.yaml - source: .github/workflows/github-release.yaml - source: .github/workflows/pre-commit.yaml - source: .github/workflows/pre-commit-optional.yaml + - source: .github/workflows/pre-commit-optional-autoupdate.yaml + - source: .github/workflows/pre-commit-autoupdate.yaml - source: .github/workflows/semantic-pull-request.yaml - source: .github/workflows/spell-check-differential.yaml pre-commands: | sd " with:\n" " with:\n local-cspell-json: .cspell.json\n" {source} - - source: .github/workflows/spell-check-differential.yaml - dest: .github/workflows/spell-check-daily.yaml + - source: .github/workflows/spell-check-daily.yaml pre-commands: | - sd "spell-check-differential" "spell-check-daily" {source} - sd " with:\n" " with:\n local-cspell-json: .cspell.json\n incremental-files-only: false\n" {source} - sd "on:\n pull_request:\n" "on:\n schedule:\n - cron: 0 0 * * *\n workflow_dispatch:\n" {source} + sd " with:\n" " with:\n local-cspell-json: .cspell.json\n" {source} - source: .github/workflows/sync-files.yaml + - source: .github/workflows/update-codeowners-from-packages.yaml + - source: docs/assets/js/mathjax.js - source: .clang-format - source: .markdown-link-check.json - source: .markdownlint.yaml - source: .pre-commit-config-optional.yaml + - source: .pre-commit-config.yaml - source: .prettierignore - source: .prettierrc.yaml - source: .yamllint.yaml + - source: CODE_OF_CONDUCT.md + - source: CONTRIBUTING.md - source: CPPLINT.cfg - - source: setup.cfg - -- repository: autowarefoundation/autoware_common - files: - - source: .github/workflows/clang-tidy-differential.yaml - pre-commands: | - sd 'container: ros:(\w+)' 'container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda' {source} - - source: .github/workflows/check-build-depends.yaml - - source: .github/workflows/clang-tidy-pr-comments.yaml - - source: .github/workflows/clang-tidy-pr-comments-manually.yaml - - source: .github/workflows/update-codeowners-from-packages.yaml - - source: .pre-commit-config.yaml - - source: codecov.yaml - -- repository: autowarefoundation/autoware-documentation - files: - - source: .github/workflows/deploy-docs.yaml - - source: .github/workflows/delete-closed-pr-docs.yaml + - source: DISCLAIMER.md + - source: LICENSE - source: mkdocs-base.yaml dest: mkdocs.yaml pre-commands: | @@ -63,4 +56,4 @@ " - macros" \ " - macros: module_name: mkdocs_macros" {source} - - source: docs/assets/js/mathjax.js + - source: setup.cfg diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 871fc67499f48..f62904b03c6e4 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -27,29 +27,58 @@ jobs: with: label: tag:require-cuda-build-and-test - build-and-test-differential: + prepare-build-and-test-differential: + runs-on: ubuntu-latest needs: [make-sure-label-is-present, make-sure-require-cuda-label-is-present] - if: ${{ needs.make-sure-label-is-present.outputs.result == 'true' }} - runs-on: ${{ matrix.runner }} - container: ${{ matrix.container }}${{ matrix.container-suffix }} - strategy: - fail-fast: false - matrix: - rosdistro: - - humble - container-suffix: - - "" - - -cuda - include: - - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:universe-devel - build-depends-repos: build_depends.repos - - container-suffix: -cuda - runner: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - build-pre-command: taskset --cpu-list 0-5 - - container-suffix: "" - runner: ubuntu-latest - build-pre-command: "" + outputs: + cuda_build: ${{ steps.check-if-cuda-build-is-required.outputs.cuda_build }} + steps: + - name: Check if cuda-build is required + id: check-if-cuda-build-is-required + run: | + if ${{ needs.make-sure-require-cuda-label-is-present.outputs.result == 'true' }}; then + echo "cuda-build is required" + echo "cuda_build=true" >> $GITHUB_OUTPUT + else + echo "cuda-build is not required" + echo "cuda_build=false" >> $GITHUB_OUTPUT + fi + shell: bash + - name: Fail if the tag:run-build-and-test-differential is missing + if: ${{ needs.make-sure-label-is-present.outputs.result != 'true' }} + run: exit 1 + + build-and-test-differential: + runs-on: ubuntu-latest + container: ghcr.io/autowarefoundation/autoware:universe-devel + needs: prepare-build-and-test-differential + steps: + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + shell: bash + + - name: Checkout PR branch and all PR commits + uses: actions/checkout@v4 + with: + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} + + - name: Run build-and-test-differential action + uses: ./.github/actions/build-and-test-differential + with: + rosdistro: humble + container: ghcr.io/autowarefoundation/autoware:universe-devel + container-suffix: "" + runner: ubuntu-latest + build-depends-repos: build_depends.repos + build-pre-command: "" + codecov-token: ${{ secrets.CODECOV_TOKEN }} + + build-and-test-differential-cuda: + runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large + container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda + needs: prepare-build-and-test-differential + if: ${{ needs.prepare-build-and-test-differential.outputs.cuda_build == 'true' }} steps: - name: Set PR fetch depth run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" @@ -62,19 +91,63 @@ jobs: fetch-depth: ${{ env.PR_FETCH_DEPTH }} - name: Run build-and-test-differential action - if: ${{ !(matrix.container-suffix == '-cuda') || needs.make-sure-require-cuda-label-is-present.outputs.result == 'true' }} uses: ./.github/actions/build-and-test-differential with: - rosdistro: ${{ matrix.rosdistro }} - container: ${{ matrix.container }} - container-suffix: ${{ matrix.container-suffix }} - runner: ${{ matrix.runner }} - build-depends-repos: ${{ matrix.build-depends-repos }} - build-pre-command: ${{ matrix.build-pre-command }} + rosdistro: humble + container: ghcr.io/autowarefoundation/autoware:universe-devel + container-suffix: -cuda + runner: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large + build-depends-repos: build_depends.repos + build-pre-command: taskset --cpu-list 0-5 codecov-token: ${{ secrets.CODECOV_TOKEN }} clang-tidy-differential: - needs: build-and-test-differential + needs: [build-and-test-differential, prepare-build-and-test-differential] + if: ${{ needs.prepare-build-and-test-differential.outputs.cuda_build == 'false' }} + runs-on: ubuntu-latest + container: ghcr.io/autowarefoundation/autoware:universe-devel + steps: + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + + - name: Checkout PR branch and all PR commits + uses: actions/checkout@v4 + with: + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} + + - name: Show disk space before the tasks + run: df -h + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 + + - name: Get modified packages + id: get-modified-packages + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Get changed files (existing files only) + id: get-changed-files + run: | + echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT + shell: bash + + - name: Run clang-tidy + if: ${{ steps.get-changed-files.outputs.changed-files != '' }} + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + with: + rosdistro: humble + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci + clang-tidy-ignore-path: .clang-tidy-ignore + build-depends-repos: build_depends.repos + cache-key-element: cuda + + - name: Show disk space after the tasks + run: df -h + + clang-tidy-differential-cuda: + needs: build-and-test-differential-cuda runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda steps: @@ -109,8 +182,8 @@ jobs: with: rosdistro: humble target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - target-files: ${{ steps.get-changed-files.outputs.changed-files }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci + clang-tidy-ignore-path: .clang-tidy-ignore build-depends-repos: build_depends.repos cache-key-element: cuda diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index bd2463d5a8eea..ee79ce0e4d41c 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: cancel-previous-workflows on: diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index 74c97e3bf4be4..bb1d89851dea6 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: check-build-depends on: @@ -20,7 +24,7 @@ jobs: build-depends-repos: build_depends.repos steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 diff --git a/.github/workflows/clang-tidy-pr-comments-manually.yaml b/.github/workflows/clang-tidy-pr-comments-manually.yaml index a4df9f9dec3ed..747c62cdcd5a3 100644 --- a/.github/workflows/clang-tidy-pr-comments-manually.yaml +++ b/.github/workflows/clang-tidy-pr-comments-manually.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: clang-tidy-pr-comments-manually on: diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index bf2ed81d7ae48..c8df6a62c9a0a 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: clang-tidy-pr-comments on: diff --git a/.github/workflows/comment-on-pr.yaml b/.github/workflows/comment-on-pr.yaml index c80141ddac6b0..0f2ecf519db11 100644 --- a/.github/workflows/comment-on-pr.yaml +++ b/.github/workflows/comment-on-pr.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: comment-on-pr on: pull_request_target: diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index 192e138a83c22..b8ff4f6d14599 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: delete-closed-pr-docs on: diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index 771b4bd36ca9d..d39e97e540794 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: deploy-docs on: diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index 4b1d7f47c6c0c..bbe2ac512d70d 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: github-release on: diff --git a/.github/workflows/pre-commit-autoupdate.yaml b/.github/workflows/pre-commit-autoupdate.yaml index 8d57a53b5ccab..489e32a1de166 100644 --- a/.github/workflows/pre-commit-autoupdate.yaml +++ b/.github/workflows/pre-commit-autoupdate.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: pre-commit-autoupdate on: diff --git a/.github/workflows/pre-commit-optional-autoupdate.yaml b/.github/workflows/pre-commit-optional-autoupdate.yaml new file mode 100644 index 0000000000000..be79ad481d16e --- /dev/null +++ b/.github/workflows/pre-commit-optional-autoupdate.yaml @@ -0,0 +1,41 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + +name: pre-commit-optional-autoupdate + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + check-secret: + uses: autowarefoundation/autoware-github-actions/.github/workflows/check-secret.yaml@v1 + secrets: + secret: ${{ secrets.APP_ID }} + + pre-commit-optional-autoupdate: + needs: check-secret + if: ${{ needs.check-secret.outputs.set == 'true' }} + runs-on: ubuntu-22.04 + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v2 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Run pre-commit-autoupdate + uses: autowarefoundation/autoware-github-actions/pre-commit-autoupdate@v1 + with: + token: ${{ steps.generate-token.outputs.token }} + pre-commit-config: .pre-commit-config-optional.yaml + pr-labels: | + tag:bot + tag:pre-commit-autoupdate + pr-branch: pre-commit-optional-autoupdate + pr-title: "ci(pre-commit-optional): autoupdate" + pr-commit-message: "ci(pre-commit-optional): autoupdate" + auto-merge-method: squash diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index 12f536c551646..3d0867028f76a 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.github/workflows/pre-commit-optional.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: pre-commit-optional on: diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index 4d005e849b5ec..15c8e86c4f950 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: pre-commit on: diff --git a/.github/workflows/semantic-pull-request.yaml b/.github/workflows/semantic-pull-request.yaml index 71224c224ec0f..b56040b084f0c 100644 --- a/.github/workflows/semantic-pull-request.yaml +++ b/.github/workflows/semantic-pull-request.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: semantic-pull-request on: diff --git a/.github/workflows/spell-check-daily.yaml b/.github/workflows/spell-check-daily.yaml index 8c0373594a90e..696963ff11cfd 100644 --- a/.github/workflows/spell-check-daily.yaml +++ b/.github/workflows/spell-check-daily.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: spell-check-daily on: @@ -18,3 +22,6 @@ jobs: local-cspell-json: .cspell.json incremental-files-only: false cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json + dict-packages: | + https://github.com/autowarefoundation/autoware-spell-check-dict + https://github.com/tier4/cspell-dicts diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml index 3542659332532..81e3309365e9b 100644 --- a/.github/workflows/spell-check-differential.yaml +++ b/.github/workflows/spell-check-differential.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: spell-check-differential on: @@ -14,4 +18,7 @@ jobs: uses: autowarefoundation/autoware-github-actions/spell-check@v1 with: local-cspell-json: .cspell.json - cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json + cspell-json-url: https://raw.githubusercontent.com/autowarefoundation/autoware-spell-check-dict/main/.cspell.json + dict-packages: | + https://github.com/autowarefoundation/autoware-spell-check-dict + https://github.com/tier4/cspell-dicts diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml index 0cffbcd2a269f..9224c1503ed22 100644 --- a/.github/workflows/sync-files.yaml +++ b/.github/workflows/sync-files.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: sync-files on: diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml index 760a647ffbf56..c9ecdb100688f 100644 --- a/.github/workflows/update-codeowners-from-packages.yaml +++ b/.github/workflows/update-codeowners-from-packages.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + name: update-codeowners-from-packages on: diff --git a/.markdownlint.yaml b/.markdownlint.yaml index 7b7359fe0cdc4..584154b2009de 100644 --- a/.markdownlint.yaml +++ b/.markdownlint.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + # See https://github.com/DavidAnson/markdownlint/blob/main/doc/Rules.md for all rules. default: true MD013: false diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index 8c9345e15f064..ff325af5e8774 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + repos: - repo: https://github.com/tcort/markdown-link-check rev: v3.12.2 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 63dc504f61a2b..6e7c64fd982fc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,9 +1,13 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + ci: autofix_commit_msg: "style(pre-commit): autofix" repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: check-json - id: check-merge-conflict @@ -18,7 +22,7 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.41.0 + rev: v0.43.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] @@ -49,7 +53,7 @@ repos: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.9.0-1 + rev: v3.10.0-2 hooks: - id: shfmt args: [-w, -s, -i=4] @@ -60,26 +64,26 @@ repos: - id: isort - repo: https://github.com/psf/black - rev: 24.8.0 + rev: 24.10.0 hooks: - id: black args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.8 + rev: v19.1.4 hooks: - id: clang-format types_or: [c++, c, cuda] - repo: https://github.com/cpplint/cpplint - rev: 1.6.1 + rev: 2.0.0 hooks: - id: cpplint args: [--quiet] exclude: .cu - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.2 + rev: 0.30.0 hooks: - id: check-metaschema files: ^.+/schema/.*schema\.json$ @@ -93,3 +97,9 @@ repos: language: node files: .svg$ additional_dependencies: [prettier@2.7.1, "@prettier/plugin-xml@2.2.0"] + + - repo: https://github.com/AleksaC/hadolint-py + rev: v2.12.1b3 + hooks: + - id: hadolint + exclude: .svg$ diff --git a/.prettierignore b/.prettierignore index a3c34d00a1377..3e96aacebba60 100644 --- a/.prettierignore +++ b/.prettierignore @@ -1,2 +1,6 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + *.param.yaml *.rviz diff --git a/.prettierrc.yaml b/.prettierrc.yaml index e29bf32762769..fe476936f7b9e 100644 --- a/.prettierrc.yaml +++ b/.prettierrc.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + printWidth: 100 tabWidth: 2 overrides: diff --git a/.yamllint.yaml b/.yamllint.yaml index 2c7bd088e2648..e0be62dbcb16f 100644 --- a/.yamllint.yaml +++ b/.yamllint.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + extends: default ignore: | diff --git a/CPPLINT.cfg b/CPPLINT.cfg index ba6bdf08c10ca..159042dba0b48 100644 --- a/CPPLINT.cfg +++ b/CPPLINT.cfg @@ -1,12 +1,18 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + # Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_cpplint/ament_cpplint/main.py#L64-L120 set noparent linelength=100 includeorder=standardcfirst filter=-build/c++11 # we do allow C++11 +filter=-build/c++17 # we allow filter=-build/namespaces_literals # we allow using namespace for literals filter=-runtime/references # we consider passing non-const references to be ok filter=-whitespace/braces # we wrap open curly braces for namespaces, classes and functions filter=-whitespace/indent # we don't indent keywords like public, protected and private with one space +filter=-whitespace/newline # we allow the developer to decide about newline at the end of file (it's clashing with clang-format) filter=-whitespace/parens # we allow closing parenthesis to be on the next line filter=-whitespace/semicolon # we allow the developer to decide about whitespace after a semicolon filter=-build/header_guard # we automatically fix the names of header guards using pre-commit diff --git a/build_depends.repos b/build_depends.repos index 61a56463e2e41..4db947b7c26a8 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -24,7 +24,7 @@ repositories: core/autoware_msgs: type: git url: https://github.com/autowarefoundation/autoware_msgs.git - version: 1.1.0 + version: 1.3.0 core/autoware_adapi_msgs: type: git url: https://github.com/autowarefoundation/autoware_adapi_msgs.git diff --git a/common/.pages b/common/.pages index 27ee85c30ea77..3c5a77c4d68cf 100644 --- a/common/.pages +++ b/common/.pages @@ -10,8 +10,8 @@ nav: - 'autoware_grid_map_utils': common/autoware_grid_map_utils - 'autoware_point_types': common/autoware_point_types - 'Geography Utils': common/autoware_geography_utils - - 'Global Parameter Loader': common/global_parameter_loader/Readme - - 'Glog Component': common/glog_component + - 'Global Parameter Loader': common/autoware_global_parameter_loader/Readme + - 'Glog Component': common/autoware_glog_component - 'interpolation': common/autoware_interpolation - 'Kalman Filter': common/autoware_kalman_filter - 'Motion Utils': common/autoware_motion_utils diff --git a/common/autoware_adapi_specs/CHANGELOG.rst b/common/autoware_adapi_specs/CHANGELOG.rst index f94b51d97f949..b4fec15bfa98c 100644 --- a/common/autoware_adapi_specs/CHANGELOG.rst +++ b/common/autoware_adapi_specs/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_ad_api_specs -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_adapi_specs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.38.0 (2024-11-08) ------------------- diff --git a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/map.hpp b/common/autoware_component_interface_specs/include/autoware/component_interface_specs/map.hpp index dd1c300a7a2ca..aeb09e44c665f 100644 --- a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/map.hpp +++ b/common/autoware_component_interface_specs/include/autoware/component_interface_specs/map.hpp @@ -17,14 +17,14 @@ #include -#include +#include namespace autoware::component_interface_specs::map { struct MapProjectorInfo { - using Message = tier4_map_msgs::msg::MapProjectorInfo; + using Message = autoware_map_msgs::msg::MapProjectorInfo; static constexpr char name[] = "/map/map_projector_info"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/autoware_component_interface_specs/package.xml b/common/autoware_component_interface_specs/package.xml index b50cd80074d1f..0fbd632d52af3 100644 --- a/common/autoware_component_interface_specs/package.xml +++ b/common/autoware_component_interface_specs/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_vehicle_msgs @@ -21,7 +22,6 @@ rosidl_runtime_cpp tier4_control_msgs tier4_localization_msgs - tier4_map_msgs tier4_planning_msgs tier4_system_msgs tier4_vehicle_msgs diff --git a/common/autoware_component_interface_tools/CMakeLists.txt b/common/autoware_component_interface_tools/CMakeLists.txt index e51db41ca0ea2..ce4af924f4c28 100644 --- a/common/autoware_component_interface_tools/CMakeLists.txt +++ b/common/autoware_component_interface_tools/CMakeLists.txt @@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "ServiceLogChecker" + PLUGIN "autoware::component_interface_tools::ServiceLogChecker" EXECUTABLE service_log_checker_node ) diff --git a/common/autoware_component_interface_tools/src/service_log_checker.cpp b/common/autoware_component_interface_tools/src/service_log_checker.cpp index 18f90af5737d2..ce413da5c1987 100644 --- a/common/autoware_component_interface_tools/src/service_log_checker.cpp +++ b/common/autoware_component_interface_tools/src/service_log_checker.cpp @@ -22,6 +22,8 @@ #define FMT_HEADER_ONLY #include +namespace autoware::component_interface_tools +{ ServiceLogChecker::ServiceLogChecker(const rclcpp::NodeOptions & options) : Node("service_log_checker", options), diagnostics_(this) { @@ -98,6 +100,6 @@ void ServiceLogChecker::update_diagnostics(diagnostic_updater::DiagnosticStatusW stat.summary(DiagnosticStatus::ERROR, "ERROR"); } } - +} // namespace autoware::component_interface_tools #include -RCLCPP_COMPONENTS_REGISTER_NODE(ServiceLogChecker) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::component_interface_tools::ServiceLogChecker) diff --git a/common/autoware_component_interface_tools/src/service_log_checker.hpp b/common/autoware_component_interface_tools/src/service_log_checker.hpp index 9579753dfd900..f6077d238e2e1 100644 --- a/common/autoware_component_interface_tools/src/service_log_checker.hpp +++ b/common/autoware_component_interface_tools/src/service_log_checker.hpp @@ -23,6 +23,8 @@ #include #include +namespace autoware::component_interface_tools +{ class ServiceLogChecker : public rclcpp::Node { public: @@ -38,5 +40,5 @@ class ServiceLogChecker : public rclcpp::Node void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); std::unordered_map errors_; }; - +} // namespace autoware::component_interface_tools #endif // SERVICE_LOG_CHECKER_HPP_ diff --git a/common/autoware_component_interface_utils/CHANGELOG.rst b/common/autoware_component_interface_utils/CHANGELOG.rst index d27f4b4558bbc..d7f476f33409a 100644 --- a/common/autoware_component_interface_utils/CHANGELOG.rst +++ b/common/autoware_component_interface_utils/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package component_interface_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_component_interface_utils +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.38.0 (2024-11-08) ------------------- diff --git a/common/autoware_component_interface_utils/test/test_component_interface_utils.cpp b/common/autoware_component_interface_utils/test/test_component_interface_utils.cpp index 7cf2dccf3e764..6919271214983 100644 --- a/common/autoware_component_interface_utils/test/test_component_interface_utils.cpp +++ b/common/autoware_component_interface_utils/test/test_component_interface_utils.cpp @@ -17,6 +17,10 @@ #include "autoware/component_interface_utils/status.hpp" #include "gtest/gtest.h" +#include +#include +#include + TEST(interface, utils) { { diff --git a/common/autoware_fake_test_node/CHANGELOG.rst b/common/autoware_fake_test_node/CHANGELOG.rst index 096c44f41ed77..330dddaa66aef 100644 --- a/common/autoware_fake_test_node/CHANGELOG.rst +++ b/common/autoware_fake_test_node/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package fake_test_node -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_fake_test_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.38.0 (2024-11-08) ------------------- diff --git a/common/autoware_geography_utils/CHANGELOG.rst b/common/autoware_geography_utils/CHANGELOG.rst deleted file mode 100644 index 623861bbd01c3..0000000000000 --- a/common/autoware_geography_utils/CHANGELOG.rst +++ /dev/null @@ -1,15 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_geography_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(geography_utils): prefix package and namespace with autoware (`#7790 `_) - * refactor(geography_utils): prefix package and namespace with autoware - * move headers to include/autoware/ - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_geography_utils/CMakeLists.txt b/common/autoware_geography_utils/CMakeLists.txt deleted file mode 100644 index b4ab5c2f74494..0000000000000 --- a/common/autoware_geography_utils/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_geography_utils) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -# GeographicLib -find_package(PkgConfig) -find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h - PATH_SUFFIXES GeographicLib -) -set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) -find_library(GeographicLib_LIBRARIES NAMES Geographic) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/height.cpp - src/projection.cpp - src/lanelet2_projector.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${GeographicLib_LIBRARIES} -) - -if(BUILD_TESTING) - find_package(ament_cmake_ros REQUIRED) - - file(GLOB_RECURSE test_files test/*.cpp) - - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - - target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME} - ) -endif() - -ament_auto_package() diff --git a/common/autoware_geography_utils/README.md b/common/autoware_geography_utils/README.md deleted file mode 100644 index fb4c2dc3a8312..0000000000000 --- a/common/autoware_geography_utils/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# geography_utils - -## Purpose - -This package contains geography-related functions used by other packages, so please refer to them as needed. diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp deleted file mode 100644 index 1f205eb8f8b18..0000000000000 --- a/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ -#define AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ - -#include - -namespace autoware::geography_utils -{ - -typedef double (*HeightConversionFunction)( - const double height, const double latitude, const double longitude); -double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude); -double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude); -double convert_height( - const double height, const double latitude, const double longitude, - const std::string & source_vertical_datum, const std::string & target_vertical_datum); - -} // namespace autoware::geography_utils - -#endif // AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp deleted file mode 100644 index 7f6b2d78701d2..0000000000000 --- a/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ -#define AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ - -#include - -#include - -#include - -namespace autoware::geography_utils -{ -using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; - -std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info); - -} // namespace autoware::geography_utils - -#endif // AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp deleted file mode 100644 index 5ad605f95f65b..0000000000000 --- a/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ -#define AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ - -#include -#include -#include - -namespace autoware::geography_utils -{ -using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; -using GeoPoint = geographic_msgs::msg::GeoPoint; -using LocalPoint = geometry_msgs::msg::Point; - -LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info); -GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info); - -} // namespace autoware::geography_utils - -#endif // AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ diff --git a/common/autoware_geography_utils/package.xml b/common/autoware_geography_utils/package.xml deleted file mode 100644 index d8ea7524eaebc..0000000000000 --- a/common/autoware_geography_utils/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - autoware_geography_utils - 0.38.0 - The autoware_geography_utils package - Yamato Ando - Masahiro Sakamoto - NGUYEN Viet Anh - Taiki Yamada - Shintaro Sakoda - Ryu Yamamoto - Apache License 2.0 - Koji Minoda - - ament_cmake_auto - autoware_cmake - - autoware_lanelet2_extension - geographic_msgs - geographiclib - geometry_msgs - lanelet2_io - tier4_map_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_geography_utils/src/height.cpp b/common/autoware_geography_utils/src/height.cpp deleted file mode 100644 index 745dbf5b22cfc..0000000000000 --- a/common/autoware_geography_utils/src/height.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include -#include -#include - -namespace autoware::geography_utils -{ - -double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude) -{ - GeographicLib::Geoid egm2008("egm2008-1"); - // cSpell: ignore ELLIPSOIDTOGEOID - return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); -} - -double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude) -{ - GeographicLib::Geoid egm2008("egm2008-1"); - // cSpell: ignore GEOIDTOELLIPSOID - return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); -} - -double convert_height( - const double height, const double latitude, const double longitude, - const std::string & source_vertical_datum, const std::string & target_vertical_datum) -{ - if (source_vertical_datum == target_vertical_datum) { - return height; - } - std::map, HeightConversionFunction> conversion_map; - conversion_map[{"WGS84", "EGM2008"}] = convert_wgs84_to_egm2008; - conversion_map[{"EGM2008", "WGS84"}] = convert_egm2008_to_wgs84; - - auto key = std::make_pair(source_vertical_datum, target_vertical_datum); - if (conversion_map.find(key) != conversion_map.end()) { - return conversion_map[key](height, latitude, longitude); - } else { - std::string error_message = - "Invalid conversion types: " + std::string(source_vertical_datum.c_str()) + " to " + - std::string(target_vertical_datum.c_str()); - - throw std::invalid_argument(error_message); - } -} - -} // namespace autoware::geography_utils diff --git a/common/autoware_geography_utils/src/lanelet2_projector.cpp b/common/autoware_geography_utils/src/lanelet2_projector.cpp deleted file mode 100644 index 7de0935a3aa4e..0000000000000 --- a/common/autoware_geography_utils/src/lanelet2_projector.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include - -namespace autoware::geography_utils -{ - -std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info) -{ - if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - lanelet::GPSPoint position{ - projector_info.map_origin.latitude, projector_info.map_origin.longitude, - projector_info.map_origin.altitude}; - lanelet::Origin origin{position}; - lanelet::projection::UtmProjector projector{origin}; - return std::make_unique(projector); - - } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { - lanelet::projection::MGRSProjector projector{}; - projector.setMGRSCode(projector_info.mgrs_grid); - return std::make_unique(projector); - - } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { - lanelet::GPSPoint position{ - projector_info.map_origin.latitude, projector_info.map_origin.longitude, - projector_info.map_origin.altitude}; - lanelet::Origin origin{position}; - lanelet::projection::TransverseMercatorProjector projector{origin}; - return std::make_unique(projector); - } - const std::string error_msg = - "Invalid map projector type: " + projector_info.projector_type + - ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator"; - throw std::invalid_argument(error_msg); -} - -} // namespace autoware::geography_utils diff --git a/common/autoware_geography_utils/src/projection.cpp b/common/autoware_geography_utils/src/projection.cpp deleted file mode 100644 index 3ab18b1d31698..0000000000000 --- a/common/autoware_geography_utils/src/projection.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -namespace autoware::geography_utils -{ - -Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) -{ - Eigen::Vector3d dst; - dst.x() = src.x; - dst.y() = src.y; - dst.z() = src.z; - return dst; -} - -LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info) -{ - std::unique_ptr projector = get_lanelet2_projector(projector_info); - lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; - - lanelet::BasicPoint3d projected_local_point; - if (projector_info.projector_type == MapProjectorInfo::MGRS) { - const int mgrs_precision = 9; // set precision as 100 micro meter - const auto mgrs_projector = dynamic_cast(projector.get()); - - // project x and y using projector - // note that the altitude is ignored in MGRS projection conventionally - projected_local_point = mgrs_projector->forward(position, mgrs_precision); - } else { - // project x and y using projector - // note that the original projector such as UTM projector does not compensate for the altitude - // offset - projected_local_point = projector->forward(position); - - // correct z based on the map origin - // note that the converted altitude in local point is in the same vertical datum as the geo - // point - projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude; - } - - LocalPoint local_point; - local_point.x = projected_local_point.x(); - local_point.y = projected_local_point.y(); - local_point.z = projected_local_point.z(); - - return local_point; -} - -GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info) -{ - std::unique_ptr projector = get_lanelet2_projector(projector_info); - - lanelet::GPSPoint projected_gps_point; - if (projector_info.projector_type == MapProjectorInfo::MGRS) { - const auto mgrs_projector = dynamic_cast(projector.get()); - // project latitude and longitude using projector - // note that the z is ignored in MGRS projection conventionally - projected_gps_point = - mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); - } else { - // project latitude and longitude using projector - // note that the original projector such as UTM projector does not compensate for the altitude - // offset - projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); - - // correct altitude based on the map origin - // note that the converted altitude in local point is in the same vertical datum as the geo - // point - projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude; - } - - GeoPoint geo_point; - geo_point.latitude = projected_gps_point.lat; - geo_point.longitude = projected_gps_point.lon; - geo_point.altitude = projected_gps_point.ele; - return geo_point; -} - -} // namespace autoware::geography_utils diff --git a/common/autoware_geography_utils/test/test_geography_utils.cpp b/common/autoware_geography_utils/test/test_geography_utils.cpp deleted file mode 100644 index ee0e7428db2f2..0000000000000 --- a/common/autoware_geography_utils/test/test_geography_utils.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/geography_utils/height.hpp" -#include "autoware/geography_utils/lanelet2_projector.hpp" -#include "autoware/geography_utils/projection.hpp" - -#include - -int main(int argc, char * argv[]) -{ - testing::InitGoogleTest(&argc, argv); - bool result = RUN_ALL_TESTS(); - return result; -} diff --git a/common/autoware_geography_utils/test/test_height.cpp b/common/autoware_geography_utils/test/test_height.cpp deleted file mode 100644 index f624f6c3ff9e3..0000000000000 --- a/common/autoware_geography_utils/test/test_height.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -#include -#include - -// Test case to verify if same source and target datums return original height -TEST(GeographyUtils, SameSourceTargetDatum) -{ - const double height = 10.0; - const double latitude = 35.0; - const double longitude = 139.0; - const std::string datum = "WGS84"; - - double converted_height = - autoware::geography_utils::convert_height(height, latitude, longitude, datum, datum); - - EXPECT_DOUBLE_EQ(height, converted_height); -} - -// Test case to verify valid source and target datums -TEST(GeographyUtils, ValidSourceTargetDatum) -{ - // Calculated with - // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html - const double height = 10.0; - const double latitude = 35.0; - const double longitude = 139.0; - const double target_height = -30.18; - - double converted_height = - autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); - - EXPECT_NEAR(target_height, converted_height, 0.1); -} - -// Test case to verify invalid source and target datums -TEST(GeographyUtils, InvalidSourceTargetDatum) -{ - const double height = 10.0; - const double latitude = 35.0; - const double longitude = 139.0; - - EXPECT_THROW( - autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), - std::invalid_argument); -} - -// Test case to verify invalid source datums -TEST(GeographyUtils, InvalidSourceDatum) -{ - const double height = 10.0; - const double latitude = 35.0; - const double longitude = 139.0; - - EXPECT_THROW( - autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), - std::invalid_argument); -} - -// Test case to verify invalid target datums -TEST(GeographyUtils, InvalidTargetDatum) -{ - const double height = 10.0; - const double latitude = 35.0; - const double longitude = 139.0; - - EXPECT_THROW( - autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), - std::invalid_argument); -} diff --git a/common/autoware_geography_utils/test/test_projection.cpp b/common/autoware_geography_utils/test/test_projection.cpp deleted file mode 100644 index 3355dbf8a50ea..0000000000000 --- a/common/autoware_geography_utils/test/test_projection.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -#include -#include - -TEST(GeographyUtilsProjection, ProjectForwardToMGRS) -{ - // source point - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.62426; - geo_point.longitude = 139.74252; - geo_point.altitude = 10.0; - - // target point - geometry_msgs::msg::Point local_point; - local_point.x = 86128.0; - local_point.y = 43002.0; - local_point.z = 10.0; - - // projector info - tier4_map_msgs::msg::MapProjectorInfo projector_info; - projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; - projector_info.mgrs_grid = "54SUE"; - projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; - - // conversion - const geometry_msgs::msg::Point converted_point = - autoware::geography_utils::project_forward(geo_point, projector_info); - - EXPECT_NEAR(converted_point.x, local_point.x, 1.0); - EXPECT_NEAR(converted_point.y, local_point.y, 1.0); - EXPECT_NEAR(converted_point.z, local_point.z, 1.0); -} - -TEST(GeographyUtilsProjection, ProjectReverseFromMGRS) -{ - // source point - geometry_msgs::msg::Point local_point; - local_point.x = 86128.0; - local_point.y = 43002.0; - local_point.z = 10.0; - - // target point - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.62426; - geo_point.longitude = 139.74252; - geo_point.altitude = 10.0; - - // projector info - tier4_map_msgs::msg::MapProjectorInfo projector_info; - projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; - projector_info.mgrs_grid = "54SUE"; - projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; - - // conversion - const geographic_msgs::msg::GeoPoint converted_point = - autoware::geography_utils::project_reverse(local_point, projector_info); - - EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001); - EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001); - EXPECT_NEAR(converted_point.altitude, geo_point.altitude, 0.0001); -} - -TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS) -{ - // source point - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.62426; - geo_point.longitude = 139.74252; - geo_point.altitude = 10.0; - - // projector info - tier4_map_msgs::msg::MapProjectorInfo projector_info; - projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; - projector_info.mgrs_grid = "54SUE"; - projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; - - // conversion - const geometry_msgs::msg::Point converted_local_point = - autoware::geography_utils::project_forward(geo_point, projector_info); - const geographic_msgs::msg::GeoPoint converted_geo_point = - autoware::geography_utils::project_reverse(converted_local_point, projector_info); - - EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); - EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); - EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); -} - -TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) -{ - // source point - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.62406; - geo_point.longitude = 139.74252; - geo_point.altitude = 10.0; - - // target point - geometry_msgs::msg::Point local_point; - local_point.x = 0.0; - local_point.y = -22.18; - local_point.z = 20.0; - - // projector info - tier4_map_msgs::msg::MapProjectorInfo projector_info; - projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; - projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; - projector_info.map_origin.latitude = 35.62426; - projector_info.map_origin.longitude = 139.74252; - projector_info.map_origin.altitude = -10.0; - - // conversion - const geometry_msgs::msg::Point converted_point = - autoware::geography_utils::project_forward(geo_point, projector_info); - - EXPECT_NEAR(converted_point.x, local_point.x, 1.0); - EXPECT_NEAR(converted_point.y, local_point.y, 1.0); - EXPECT_NEAR(converted_point.z, local_point.z, 1.0); -} - -TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) -{ - // source point - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.62426; - geo_point.longitude = 139.74252; - geo_point.altitude = 10.0; - - // projector info - tier4_map_msgs::msg::MapProjectorInfo projector_info; - projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; - projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; - projector_info.map_origin.latitude = 35.0; - projector_info.map_origin.longitude = 139.0; - projector_info.map_origin.altitude = 0.0; - - // conversion - const geometry_msgs::msg::Point converted_local_point = - autoware::geography_utils::project_forward(geo_point, projector_info); - const geographic_msgs::msg::GeoPoint converted_geo_point = - autoware::geography_utils::project_reverse(converted_local_point, projector_info); - - EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); - EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); - EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); -} diff --git a/common/global_parameter_loader/CHANGELOG.rst b/common/autoware_global_parameter_loader/CHANGELOG.rst similarity index 100% rename from common/global_parameter_loader/CHANGELOG.rst rename to common/autoware_global_parameter_loader/CHANGELOG.rst diff --git a/common/global_parameter_loader/CMakeLists.txt b/common/autoware_global_parameter_loader/CMakeLists.txt similarity index 77% rename from common/global_parameter_loader/CMakeLists.txt rename to common/autoware_global_parameter_loader/CMakeLists.txt index e67ae1a5c06fc..fe47ffc0c845b 100644 --- a/common/global_parameter_loader/CMakeLists.txt +++ b/common/autoware_global_parameter_loader/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(global_parameter_loader) +project(autoware_global_parameter_loader) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/common/global_parameter_loader/Readme.md b/common/autoware_global_parameter_loader/Readme.md similarity index 83% rename from common/global_parameter_loader/Readme.md rename to common/autoware_global_parameter_loader/Readme.md index 53cb9e11e096f..7b818c26a87f2 100644 --- a/common/global_parameter_loader/Readme.md +++ b/common/autoware_global_parameter_loader/Readme.md @@ -8,7 +8,7 @@ Add the following lines to the launch file of the node in which you want to get ```xml - + ``` diff --git a/common/global_parameter_loader/launch/global_params.launch.py b/common/autoware_global_parameter_loader/launch/global_params.launch.py similarity index 100% rename from common/global_parameter_loader/launch/global_params.launch.py rename to common/autoware_global_parameter_loader/launch/global_params.launch.py diff --git a/common/global_parameter_loader/package.xml b/common/autoware_global_parameter_loader/package.xml similarity index 84% rename from common/global_parameter_loader/package.xml rename to common/autoware_global_parameter_loader/package.xml index 64183edaa6b1a..2a72f93230da9 100644 --- a/common/global_parameter_loader/package.xml +++ b/common/autoware_global_parameter_loader/package.xml @@ -1,9 +1,9 @@ - global_parameter_loader + autoware_global_parameter_loader 0.38.0 - The global_parameter_loader package + The autoware_global_parameter_loader package Ryohsuke Mitsudome Apache License 2.0 diff --git a/common/glog_component/CHANGELOG.rst b/common/autoware_glog_component/CHANGELOG.rst similarity index 100% rename from common/glog_component/CHANGELOG.rst rename to common/autoware_glog_component/CHANGELOG.rst diff --git a/common/autoware_glog_component/CMakeLists.txt b/common/autoware_glog_component/CMakeLists.txt new file mode 100644 index 0000000000000..37e2862517bc1 --- /dev/null +++ b/common/autoware_glog_component/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_glog_component) + +find_package(autoware_cmake REQUIRED) +autoware_package() + + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/glog_component.cpp +) +target_link_libraries(${PROJECT_NAME} glog::glog) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::glog_component::GlogComponent" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package() diff --git a/common/glog_component/README.md b/common/autoware_glog_component/README.md similarity index 88% rename from common/glog_component/README.md rename to common/autoware_glog_component/README.md index 94a40fe32e40d..ae8d8e5b9b8f2 100644 --- a/common/glog_component/README.md +++ b/common/autoware_glog_component/README.md @@ -10,8 +10,8 @@ When you load the `glog_component` in container, the launch file can be like bel ```py glog_component = ComposableNode( - package="glog_component", - plugin="GlogComponent", + package="autoware_glog_component", + plugin="autoware::glog_component::GlogComponent", name="glog_component", ) diff --git a/common/glog_component/include/glog_component/glog_component.hpp b/common/autoware_glog_component/include/autoware/glog_component/glog_component.hpp similarity index 75% rename from common/glog_component/include/glog_component/glog_component.hpp rename to common/autoware_glog_component/include/autoware/glog_component/glog_component.hpp index d940363d75ac4..8e0a45d4e32c8 100644 --- a/common/glog_component/include/glog_component/glog_component.hpp +++ b/common/autoware_glog_component/include/autoware/glog_component/glog_component.hpp @@ -12,17 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GLOG_COMPONENT__GLOG_COMPONENT_HPP_ -#define GLOG_COMPONENT__GLOG_COMPONENT_HPP_ +#ifndef AUTOWARE__GLOG_COMPONENT__GLOG_COMPONENT_HPP_ +#define AUTOWARE__GLOG_COMPONENT__GLOG_COMPONENT_HPP_ #include #include +namespace autoware::glog_component +{ + class GlogComponent : public rclcpp::Node { public: explicit GlogComponent(const rclcpp::NodeOptions & node_options); }; -#endif // GLOG_COMPONENT__GLOG_COMPONENT_HPP_ +} // namespace autoware::glog_component + +#endif // AUTOWARE__GLOG_COMPONENT__GLOG_COMPONENT_HPP_ diff --git a/common/glog_component/package.xml b/common/autoware_glog_component/package.xml similarity index 87% rename from common/glog_component/package.xml rename to common/autoware_glog_component/package.xml index a9e7cdf54fd5a..f9e6a1aa26723 100644 --- a/common/glog_component/package.xml +++ b/common/autoware_glog_component/package.xml @@ -1,9 +1,9 @@ - glog_component + autoware_glog_component 0.38.0 - The glog_component package + The autoware_glog_component package Takamasa Horibe Apache License 2.0 diff --git a/common/glog_component/src/glog_component.cpp b/common/autoware_glog_component/src/glog_component.cpp similarity index 81% rename from common/glog_component/src/glog_component.cpp rename to common/autoware_glog_component/src/glog_component.cpp index 7fd9c923d23c9..cddfd206668be 100644 --- a/common/glog_component/src/glog_component.cpp +++ b/common/autoware_glog_component/src/glog_component.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "glog_component/glog_component.hpp" +#include "autoware/glog_component/glog_component.hpp" + +namespace autoware::glog_component +{ GlogComponent::GlogComponent(const rclcpp::NodeOptions & node_options) : Node("glog_component", node_options) @@ -23,5 +26,7 @@ GlogComponent::GlogComponent(const rclcpp::NodeOptions & node_options) } } +} // namespace autoware::glog_component + #include -RCLCPP_COMPONENTS_REGISTER_NODE(GlogComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::glog_component::GlogComponent) diff --git a/common/autoware_grid_map_utils/src/polygon_iterator.cpp b/common/autoware_grid_map_utils/src/polygon_iterator.cpp index d3c5de85c2ed2..47ff9a9e5957f 100644 --- a/common/autoware_grid_map_utils/src/polygon_iterator.cpp +++ b/common/autoware_grid_map_utils/src/polygon_iterator.cpp @@ -21,6 +21,7 @@ #include #include #include +#include namespace autoware::grid_map_utils { diff --git a/common/autoware_interpolation/src/spherical_linear_interpolation.cpp b/common/autoware_interpolation/src/spherical_linear_interpolation.cpp index 697dda3b06770..5acdf6a9036a3 100644 --- a/common/autoware_interpolation/src/spherical_linear_interpolation.cpp +++ b/common/autoware_interpolation/src/spherical_linear_interpolation.cpp @@ -14,6 +14,8 @@ #include "autoware/interpolation/spherical_linear_interpolation.hpp" +#include + namespace autoware::interpolation { geometry_msgs::msg::Quaternion slerp( diff --git a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp index 31c1c768d7173..4d1dd8f33b7a6 100644 --- a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp +++ b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp @@ -14,6 +14,8 @@ #include "autoware/kalman_filter/time_delay_kalman_filter.hpp" +#include + namespace autoware::kalman_filter { TimeDelayKalmanFilter::TimeDelayKalmanFilter() diff --git a/common/autoware_motion_utils/src/distance/distance.cpp b/common/autoware_motion_utils/src/distance/distance.cpp index d31e7ec709810..c218561beacec 100644 --- a/common/autoware_motion_utils/src/distance/distance.cpp +++ b/common/autoware_motion_utils/src/distance/distance.cpp @@ -14,6 +14,8 @@ #include "autoware/motion_utils/distance/distance.hpp" +#include + namespace autoware::motion_utils { namespace diff --git a/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp b/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp index f85b5b26cd0c2..7f12e2972ec86 100644 --- a/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp +++ b/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp @@ -14,6 +14,8 @@ #include +#include + namespace autoware::motion_utils { void SteeringFactorInterface::set( diff --git a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp index 6a1ddff453bd0..cfa3c91eac43e 100644 --- a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp @@ -19,6 +19,9 @@ #include #include +#include +#include + namespace autoware::motion_utils { template diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp index 29462afcb5b76..91ad1c41eecc3 100644 --- a/common/autoware_motion_utils/src/marker/marker_helper.cpp +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -20,6 +20,8 @@ #include +#include + using autoware::universe_utils::createDefaultMarker; using autoware::universe_utils::createDeletedDefaultMarker; using autoware::universe_utils::createMarkerColor; diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index 88ebf3f41c408..9e736444495fa 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -21,7 +21,10 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include #include +#include +#include namespace autoware::motion_utils { diff --git a/common/autoware_motion_utils/src/trajectory/conversion.cpp b/common/autoware_motion_utils/src/trajectory/conversion.cpp index 368d3e0fbbb75..706dd6dae79af 100644 --- a/common/autoware_motion_utils/src/trajectory/conversion.cpp +++ b/common/autoware_motion_utils/src/trajectory/conversion.cpp @@ -15,6 +15,7 @@ #include "autoware/motion_utils/trajectory/conversion.hpp" #include +#include namespace autoware::motion_utils { diff --git a/common/autoware_motion_utils/src/trajectory/path_shift.cpp b/common/autoware_motion_utils/src/trajectory/path_shift.cpp index 6f57f34a56190..8864128f94bd9 100644 --- a/common/autoware_motion_utils/src/trajectory/path_shift.cpp +++ b/common/autoware_motion_utils/src/trajectory/path_shift.cpp @@ -16,6 +16,9 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include +#include + namespace autoware::motion_utils { double calc_feasible_velocity_from_jerk( diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index 080d8ca8c7437..35e39fda75a69 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -14,6 +14,10 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include +#include +#include + namespace autoware::motion_utils { diff --git a/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp index 5e2e0cc4bdf02..afce7f953e644 100644 --- a/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp @@ -14,6 +14,9 @@ #include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" #include "gtest/gtest.h" + +#include + namespace { constexpr auto wall_ns_suffix = "_virtual_wall"; diff --git a/common/autoware_motion_utils/test/src/resample/test_resample.cpp b/common/autoware_motion_utils/test/src/resample/test_resample.cpp index 68ead4b8c1520..ef97dfa410d2d 100644 --- a/common/autoware_motion_utils/test/src/resample/test_resample.cpp +++ b/common/autoware_motion_utils/test/src/resample/test_resample.cpp @@ -24,6 +24,7 @@ #include #include +#include namespace { diff --git a/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp index 4ff9b2a33ca13..f34365b08a943 100644 --- a/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include diff --git a/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp b/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp index 196f82b69161a..55ac4e2a370c3 100644 --- a/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp @@ -19,6 +19,7 @@ #include "autoware/interpolation/spline_interpolation.hpp" #include +#include namespace autoware::object_recognition_utils { diff --git a/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp b/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp index 1482158581a8e..31d2a90f2e4a5 100644 --- a/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp @@ -16,6 +16,8 @@ #include +#include + constexpr double epsilon = 1e-06; namespace diff --git a/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp index a93d253bfd052..1f434d3362b86 100644 --- a/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -20,6 +20,8 @@ #include +#include + using autoware::universe_utils::Point2d; using autoware::universe_utils::Point3d; diff --git a/common/autoware_osqp_interface/CMakeLists.txt b/common/autoware_osqp_interface/CMakeLists.txt index e9ed0ce25f2f8..b770af659e52a 100644 --- a/common/autoware_osqp_interface/CMakeLists.txt +++ b/common/autoware_osqp_interface/CMakeLists.txt @@ -26,7 +26,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ${OSQP_INTERFACE_LIB_SRC} ${OSQP_INTERFACE_LIB_HEADERS} ) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast) +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC diff --git a/common/autoware_osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp index 9ebf132f65028..ceeae626222ca 100644 --- a/common/autoware_osqp_interface/src/osqp_interface.cpp +++ b/common/autoware_osqp_interface/src/osqp_interface.cpp @@ -430,6 +430,6 @@ void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const output_message += "Optimization failed due to " + status_message; // log with warning - RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); + RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), "%s", output_message.c_str()); } } // namespace autoware::osqp_interface diff --git a/common/autoware_osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp index d03713f82566f..f65b07e6d792d 100644 --- a/common/autoware_osqp_interface/test/test_osqp_interface.cpp +++ b/common/autoware_osqp_interface/test/test_osqp_interface.cpp @@ -17,6 +17,7 @@ #include +#include #include #include diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp index 68813a5f1140f..7989dd21d8b2e 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp @@ -52,6 +52,8 @@ #include +#include + namespace autoware::mission_details_overlay_rviz_plugin { ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp index 13f3fdddfe76b..41ee64f5233a7 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include 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 03729d122e614..12e25ee3bb954 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 @@ -28,6 +28,7 @@ #include #include #include +#include #include #include diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp index d6f061e724369..076b92a415ee0 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp @@ -52,6 +52,8 @@ #include +#include + namespace autoware_overlay_rviz_plugin { ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) 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 d677be40dead2..faf9c39af672d 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 @@ -28,6 +28,7 @@ #include #include #include +#include #include #include 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 1da64aaf320ca..36b5212c08f75 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 @@ -29,6 +29,7 @@ #include #include #include +#include #include #include 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 28bd2b5c2a52e..d475a8231e595 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 @@ -28,6 +28,7 @@ #include #include #include +#include #include #include 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 b42b5d953fcc6..6748f01fe139b 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 @@ -28,6 +28,7 @@ #include #include #include +#include #include #include diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index d11aba912854d..b95b94248c943 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -16,6 +16,7 @@ #include #include +#include namespace autoware { diff --git a/common/autoware_pyplot/README.md b/common/autoware_pyplot/README.md new file mode 100644 index 0000000000000..ded3387fefd99 --- /dev/null +++ b/common/autoware_pyplot/README.md @@ -0,0 +1,43 @@ +# autoware_pyplot + +This package provides C++ interface for the notable `matplotlib` using `pybind11` backend for + +- creating scientific plots and images illustrating the function inputs/outputs +- debugging the output and internal data of a function before unit testing in a more lightweight manner than planning_simulator + +## usage + +In your main function, setup the python context and import `matplotlib` + +```cpp +#include +#include + +// in main... + py::scoped_interpreter guard{}; + auto plt = autoware::pyplot::import(); +``` + +Then you can use major functionalities of `matplotlib` almost in the same way as native python code. + +```cpp +{ + plt.plot(Args(std::vector({1, 3, 2, 4})), Kwargs("color"_a = "blue", "linewidth"_a = 1.0)); + plt.xlabel(Args("x-title")); + plt.ylabel(Args("y-title")); + plt.title(Args("title")); + plt.xlim(Args(0, 5)); + plt.ylim(Args(0, 5)); + plt.grid(Args(true)); + plt.savefig(Args("test_single_plot.png")); +} + +{ + auto [fig, axes] = plt.subplots(1, 2); + auto & ax1 = axes[0]; + auto & ax2 = axes[1]; + + ax1.set_aspect(Args("equal")); + ax2.set_aspect(Args("equal")); +} +``` diff --git a/common/autoware_pyplot/src/axes.cpp b/common/autoware_pyplot/src/axes.cpp index 0f08ffe5773f6..95f871ea6179f 100644 --- a/common/autoware_pyplot/src/axes.cpp +++ b/common/autoware_pyplot/src/axes.cpp @@ -15,6 +15,8 @@ #include #include +#include + namespace autoware::pyplot { inline namespace axes diff --git a/common/autoware_pyplot/src/common.cpp b/common/autoware_pyplot/src/common.cpp index 5b3fffa37f30e..5f1226cb5bae1 100644 --- a/common/autoware_pyplot/src/common.cpp +++ b/common/autoware_pyplot/src/common.cpp @@ -14,6 +14,8 @@ #include +#include + namespace autoware::pyplot { inline namespace common diff --git a/common/autoware_pyplot/src/pyplot.cpp b/common/autoware_pyplot/src/pyplot.cpp index d45d34a35d767..624a1b1db07df 100644 --- a/common/autoware_pyplot/src/pyplot.cpp +++ b/common/autoware_pyplot/src/pyplot.cpp @@ -15,6 +15,9 @@ #include #include +#include +#include + namespace autoware::pyplot { PyPlot::PyPlot(const pybind11::module & mod_) : mod(mod_) diff --git a/common/autoware_qp_interface/CHANGELOG.rst b/common/autoware_qp_interface/CHANGELOG.rst index b2e26a8e0085e..17e296bab4fca 100644 --- a/common/autoware_qp_interface/CHANGELOG.rst +++ b/common/autoware_qp_interface/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package qp_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_qp_interface +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.38.0 (2024-11-08) ------------------- diff --git a/common/autoware_qp_interface/CMakeLists.txt b/common/autoware_qp_interface/CMakeLists.txt index d2f2d6347cb3d..1df75d2d719a0 100644 --- a/common/autoware_qp_interface/CMakeLists.txt +++ b/common/autoware_qp_interface/CMakeLists.txt @@ -30,7 +30,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ${QP_INTERFACE_LIB_SRC} ${QP_INTERFACE_LIB_HEADERS} ) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast) +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC diff --git a/common/autoware_qp_interface/src/osqp_interface.cpp b/common/autoware_qp_interface/src/osqp_interface.cpp index b850036c0aebf..fbb8e71f4c251 100644 --- a/common/autoware_qp_interface/src/osqp_interface.cpp +++ b/common/autoware_qp_interface/src/osqp_interface.cpp @@ -18,6 +18,11 @@ #include +#include +#include +#include +#include + namespace autoware::qp_interface { OSQPInterface::OSQPInterface( diff --git a/common/autoware_qp_interface/src/proxqp_interface.cpp b/common/autoware_qp_interface/src/proxqp_interface.cpp index ccfd74e70dace..a0ebbf0db0510 100644 --- a/common/autoware_qp_interface/src/proxqp_interface.cpp +++ b/common/autoware_qp_interface/src/proxqp_interface.cpp @@ -14,6 +14,10 @@ #include "autoware/qp_interface/proxqp_interface.hpp" +#include +#include +#include + namespace autoware::qp_interface { using proxsuite::proxqp::QPSolverOutput; diff --git a/common/autoware_qp_interface/test/test_osqp_interface.cpp b/common/autoware_qp_interface/test/test_osqp_interface.cpp index 873e5d501dc7d..f97cc2888afa0 100644 --- a/common/autoware_qp_interface/test/test_osqp_interface.cpp +++ b/common/autoware_qp_interface/test/test_osqp_interface.cpp @@ -17,6 +17,8 @@ #include +#include +#include #include #include diff --git a/common/autoware_qp_interface/test/test_proxqp_interface.cpp b/common/autoware_qp_interface/test/test_proxqp_interface.cpp index 5bf85b026478d..e47af10c7aabd 100644 --- a/common/autoware_qp_interface/test/test_proxqp_interface.cpp +++ b/common/autoware_qp_interface/test/test_proxqp_interface.cpp @@ -17,6 +17,8 @@ #include +#include +#include #include #include diff --git a/common/autoware_signal_processing/src/butterworth.cpp b/common/autoware_signal_processing/src/butterworth.cpp index 7f491d443787b..0143b7b4fe273 100644 --- a/common/autoware_signal_processing/src/butterworth.cpp +++ b/common/autoware_signal_processing/src/butterworth.cpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace autoware::signal_processing { diff --git a/common/autoware_signal_processing/test/src/butterworth_filter_test.cpp b/common/autoware_signal_processing/test/src/butterworth_filter_test.cpp index bd5d1dbbf6948..8fa72676b02cc 100644 --- a/common/autoware_signal_processing/test/src/butterworth_filter_test.cpp +++ b/common/autoware_signal_processing/test/src/butterworth_filter_test.cpp @@ -14,6 +14,8 @@ #include "butterworth_filter_test.hpp" +#include + using autoware::signal_processing::ButterworthFilter; TEST_F(ButterWorthTestFixture, butterworthOrderTest) diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp index 5d36e8aefef3b..5e07237e2c495 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -197,10 +198,10 @@ template T parse(const std::string & filename); template <> -LaneletRoute parse(const std::string & filename); +std::optional parse(const std::string & filename); template <> -PathWithLaneId parse(const std::string & filename); +std::optional parse(const std::string & filename); template auto create_const_shared_ptr(MessageType && payload) diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp index 696f58d5e19e8..d42a5219e374c 100644 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -26,7 +26,9 @@ #include #include +#include #include +#include namespace autoware::test_utils { @@ -310,7 +312,12 @@ PathWithLaneId loadPathWithLaneIdInYaml() const auto yaml_path = get_absolute_path_to_config("autoware_test_utils", "path_with_lane_id_data.yaml"); - return parse(yaml_path); + if (const auto path = parse>(yaml_path)) { + return *path; + } + + throw std::runtime_error( + "Failed to parse YAML file: " + yaml_path + ". The file might be corrupted."); } lanelet::ConstLanelet make_lanelet( diff --git a/common/autoware_test_utils/src/mock_data_parser.cpp b/common/autoware_test_utils/src/mock_data_parser.cpp index 9e895c7b4027d..6e7002501bf30 100644 --- a/common/autoware_test_utils/src/mock_data_parser.cpp +++ b/common/autoware_test_utils/src/mock_data_parser.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -220,11 +221,7 @@ std::vector parse>(const Y { std::vector path_points; - if (!node["points"]) { - return path_points; - } - - const auto & points = node["points"]; + const auto & points = node; path_points.reserve(points.size()); std::transform( points.begin(), points.end(), std::back_inserter(path_points), [&](const YAML::Node & input) { @@ -428,36 +425,30 @@ OperationModeState parse(const YAML::Node & node) } template <> -LaneletRoute parse(const std::string & filename) +std::optional parse(const std::string & filename) { - LaneletRoute lanelet_route; - try { - YAML::Node config = YAML::LoadFile(filename); - - lanelet_route.start_pose = (config["start_pose"]) ? parse(config["start_pose"]) : Pose(); - lanelet_route.goal_pose = (config["goal_pose"]) ? parse(config["goal_pose"]) : Pose(); - lanelet_route.segments = parse>(config["segments"]); - } catch (const std::exception & e) { - RCLCPP_DEBUG(rclcpp::get_logger("autoware_test_utils"), "Exception caught: %s", e.what()); + YAML::Node node = YAML::LoadFile(filename); + if (!node["start_pose"] || !node["goal_pose"] || !node["segments"]) { + return std::nullopt; } - return lanelet_route; + + return parse(node); } template <> -PathWithLaneId parse(const std::string & filename) +std::optional parse(const std::string & filename) { - PathWithLaneId path; - YAML::Node yaml_node = YAML::LoadFile(filename); - - try { - path.header = parse
(yaml_node["header"]); - path.points = parse>(yaml_node); - path.left_bound = parse>(yaml_node["left_bound"]); - path.right_bound = parse>(yaml_node["right_bound"]); - } catch (const std::exception & e) { - RCLCPP_DEBUG(rclcpp::get_logger("autoware_test_utils"), "Exception caught: %s", e.what()); + YAML::Node node = YAML::LoadFile(filename); + + if (!node["header"] || !node["points"] || !node["left_bound"] || !node["right_bound"]) { + return std::nullopt; } + PathWithLaneId path; + path.header = parse
(node["header"]); + path.points = parse>(node["points"]); + path.left_bound = parse>(node["left_bound"]); + path.right_bound = parse>(node["right_bound"]); return path; } } // namespace autoware::test_utils diff --git a/common/autoware_test_utils/src/topic_snapshot_saver.cpp b/common/autoware_test_utils/src/topic_snapshot_saver.cpp index 82700be9e3f54..b2cb2a17c9621 100644 --- a/common/autoware_test_utils/src/topic_snapshot_saver.cpp +++ b/common/autoware_test_utils/src/topic_snapshot_saver.cpp @@ -29,10 +29,15 @@ #include #include +#include +#include #include #include +#include #include +#include #include +#include using MessageType = std::variant< nav_msgs::msg::Odometry, // 0 diff --git a/common/autoware_test_utils/test/test_autoware_test_manager.cpp b/common/autoware_test_utils/test/test_autoware_test_manager.cpp index 688ec9df9ad42..9e0e00d199f67 100644 --- a/common/autoware_test_utils/test/test_autoware_test_manager.cpp +++ b/common/autoware_test_utils/test/test_autoware_test_manager.cpp @@ -20,6 +20,9 @@ #include +#include +#include + class RelayNode : public rclcpp::Node { public: diff --git a/common/autoware_test_utils/test/test_mock_data_parser.cpp b/common/autoware_test_utils/test/test_mock_data_parser.cpp index a0dd0fce9bd29..036f16c827713 100644 --- a/common/autoware_test_utils/test/test_mock_data_parser.cpp +++ b/common/autoware_test_utils/test/test_mock_data_parser.cpp @@ -20,6 +20,8 @@ #include +#include + namespace autoware::test_utils { using tier4_planning_msgs::msg::PathWithLaneId; @@ -671,11 +673,12 @@ TEST(ParseFunctions, CompleteYAMLTest) // Test parsing of segments const auto segments = parse>(config["segments"]); ASSERT_EQ( - segments.size(), uint64_t(1)); // Assuming only one segment in the provided YAML for this test + segments.size(), + static_cast(1)); // Assuming only one segment in the provided YAML for this test const auto & segment0 = segments[0]; EXPECT_EQ(segment0.preferred_primitive.id, 11); - EXPECT_EQ(segment0.primitives.size(), uint64_t(2)); + EXPECT_EQ(segment0.primitives.size(), static_cast(2)); EXPECT_EQ(segment0.primitives[0].id, 22); EXPECT_EQ(segment0.primitives[0].primitive_type, "lane"); EXPECT_EQ(segment0.primitives[1].id, 33); @@ -788,38 +791,44 @@ TEST(ParseFunction, CompleteFromFilename) const auto parser_test_route = autoware_test_utils_dir + "/test_data/lanelet_route_parser_test.yaml"; - const auto lanelet_route = parse(parser_test_route); - EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.x, 1.0); - EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.y, 2.0); - EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.z, 3.0); + if (const auto lanelet_route_opt = parse>(parser_test_route)) { + const auto & lanelet_route = *lanelet_route_opt; + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.y, 2.0); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.z, 3.0); - EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.x, 0.1); - EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.y, 0.2); - EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.z, 0.3); - EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.w, 0.4); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.x, 0.1); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.y, 0.2); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.z, 0.3); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.w, 0.4); - EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.x, 4.0); - EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.y, 5.0); - EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.z, 6.0); - EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.x, 0.5); - EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.y, 0.6); - EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.z, 0.7); - EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.w, 0.8); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.x, 4.0); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.y, 5.0); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.z, 6.0); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.x, 0.5); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.y, 0.6); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.z, 0.7); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.w, 0.8); - ASSERT_EQ( - lanelet_route.segments.size(), - uint64_t(2)); // Assuming only one segment in the provided YAML for this test - const auto & segment1 = lanelet_route.segments[1]; - EXPECT_EQ(segment1.preferred_primitive.id, 44); - EXPECT_EQ(segment1.primitives.size(), uint64_t(4)); - EXPECT_EQ(segment1.primitives[0].id, 55); - EXPECT_EQ(segment1.primitives[0].primitive_type, "lane"); - EXPECT_EQ(segment1.primitives[1].id, 66); - EXPECT_EQ(segment1.primitives[1].primitive_type, "lane"); - EXPECT_EQ(segment1.primitives[2].id, 77); - EXPECT_EQ(segment1.primitives[2].primitive_type, "lane"); - EXPECT_EQ(segment1.primitives[3].id, 88); - EXPECT_EQ(segment1.primitives[3].primitive_type, "lane"); + ASSERT_EQ( + lanelet_route.segments.size(), + static_cast(2)); // Assuming only one segment in the provided YAML for this test + const auto & segment1 = lanelet_route.segments[1]; + EXPECT_EQ(segment1.preferred_primitive.id, 44); + EXPECT_EQ(segment1.primitives.size(), static_cast(4)); + EXPECT_EQ(segment1.primitives[0].id, 55); + EXPECT_EQ(segment1.primitives[0].primitive_type, "lane"); + EXPECT_EQ(segment1.primitives[1].id, 66); + EXPECT_EQ(segment1.primitives[1].primitive_type, "lane"); + EXPECT_EQ(segment1.primitives[2].id, 77); + EXPECT_EQ(segment1.primitives[2].primitive_type, "lane"); + EXPECT_EQ(segment1.primitives[3].id, 88); + EXPECT_EQ(segment1.primitives[3].primitive_type, "lane"); + } else { + const std::string fail_reason = + "Failed to parse YAML file: " + parser_test_route + ". The file might be corrupted."; + FAIL() << fail_reason; + } } TEST(ParseFunction, ParsePathWithLaneID) @@ -829,45 +838,49 @@ TEST(ParseFunction, ParsePathWithLaneID) const auto parser_test_path = autoware_test_utils_dir + "/test_data/path_with_lane_id_parser_test.yaml"; - const auto path = parse(parser_test_path); - EXPECT_EQ(path.header.stamp.sec, 20); - EXPECT_EQ(path.header.stamp.nanosec, 5); + if (const auto path_opt = parse>(parser_test_path)) { + const auto & path = *path_opt; + EXPECT_EQ(path.header.stamp.sec, 20); + EXPECT_EQ(path.header.stamp.nanosec, 5); - const auto path_points = path.points; - const auto & p1 = path_points.front(); - EXPECT_DOUBLE_EQ(p1.point.pose.position.x, 12.9); - EXPECT_DOUBLE_EQ(p1.point.pose.position.y, 3.8); - EXPECT_DOUBLE_EQ(p1.point.pose.position.z, 4.7); - EXPECT_DOUBLE_EQ(p1.point.pose.orientation.x, 1.0); - EXPECT_DOUBLE_EQ(p1.point.pose.orientation.y, 2.0); - EXPECT_DOUBLE_EQ(p1.point.pose.orientation.z, 3.0); - EXPECT_DOUBLE_EQ(p1.point.pose.orientation.w, 4.0); - EXPECT_FLOAT_EQ(p1.point.longitudinal_velocity_mps, 1.2); - EXPECT_FLOAT_EQ(p1.point.lateral_velocity_mps, 3.4); - EXPECT_FLOAT_EQ(p1.point.heading_rate_rps, 5.6); - EXPECT_TRUE(p1.point.is_final); - EXPECT_EQ(p1.lane_ids.front(), 912); + const auto path_points = path.points; + const auto & p1 = path_points.front(); + EXPECT_DOUBLE_EQ(p1.point.pose.position.x, 12.9); + EXPECT_DOUBLE_EQ(p1.point.pose.position.y, 3.8); + EXPECT_DOUBLE_EQ(p1.point.pose.position.z, 4.7); + EXPECT_DOUBLE_EQ(p1.point.pose.orientation.x, 1.0); + EXPECT_DOUBLE_EQ(p1.point.pose.orientation.y, 2.0); + EXPECT_DOUBLE_EQ(p1.point.pose.orientation.z, 3.0); + EXPECT_DOUBLE_EQ(p1.point.pose.orientation.w, 4.0); + EXPECT_FLOAT_EQ(p1.point.longitudinal_velocity_mps, 1.2); + EXPECT_FLOAT_EQ(p1.point.lateral_velocity_mps, 3.4); + EXPECT_FLOAT_EQ(p1.point.heading_rate_rps, 5.6); + EXPECT_TRUE(p1.point.is_final); + EXPECT_EQ(p1.lane_ids.front(), 912); - const auto & p2 = path_points.back(); - EXPECT_DOUBLE_EQ(p2.point.pose.position.x, 0.0); - EXPECT_DOUBLE_EQ(p2.point.pose.position.y, 20.5); - EXPECT_DOUBLE_EQ(p2.point.pose.position.z, 90.11); - EXPECT_DOUBLE_EQ(p2.point.pose.orientation.x, 4.0); - EXPECT_DOUBLE_EQ(p2.point.pose.orientation.y, 3.0); - EXPECT_DOUBLE_EQ(p2.point.pose.orientation.z, 2.0); - EXPECT_DOUBLE_EQ(p2.point.pose.orientation.w, 1.0); - EXPECT_FLOAT_EQ(p2.point.longitudinal_velocity_mps, 2.1); - EXPECT_FLOAT_EQ(p2.point.lateral_velocity_mps, 4.3); - EXPECT_FLOAT_EQ(p2.point.heading_rate_rps, 6.5); - EXPECT_FALSE(p2.point.is_final); - EXPECT_EQ(p2.lane_ids.front(), 205); + const auto & p2 = path_points.back(); + EXPECT_DOUBLE_EQ(p2.point.pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(p2.point.pose.position.y, 20.5); + EXPECT_DOUBLE_EQ(p2.point.pose.position.z, 90.11); + EXPECT_DOUBLE_EQ(p2.point.pose.orientation.x, 4.0); + EXPECT_DOUBLE_EQ(p2.point.pose.orientation.y, 3.0); + EXPECT_DOUBLE_EQ(p2.point.pose.orientation.z, 2.0); + EXPECT_DOUBLE_EQ(p2.point.pose.orientation.w, 1.0); + EXPECT_FLOAT_EQ(p2.point.longitudinal_velocity_mps, 2.1); + EXPECT_FLOAT_EQ(p2.point.lateral_velocity_mps, 4.3); + EXPECT_FLOAT_EQ(p2.point.heading_rate_rps, 6.5); + EXPECT_FALSE(p2.point.is_final); + EXPECT_EQ(p2.lane_ids.front(), 205); - EXPECT_DOUBLE_EQ(path.left_bound.front().x, 55.0); - EXPECT_DOUBLE_EQ(path.left_bound.front().y, 66.0); - EXPECT_DOUBLE_EQ(path.left_bound.front().z, 77.0); + EXPECT_DOUBLE_EQ(path.left_bound.front().x, 55.0); + EXPECT_DOUBLE_EQ(path.left_bound.front().y, 66.0); + EXPECT_DOUBLE_EQ(path.left_bound.front().z, 77.0); - EXPECT_DOUBLE_EQ(path.right_bound.front().x, 0.55); - EXPECT_DOUBLE_EQ(path.right_bound.front().y, 0.66); - EXPECT_DOUBLE_EQ(path.right_bound.front().z, 0.77); + EXPECT_DOUBLE_EQ(path.right_bound.front().x, 0.55); + EXPECT_DOUBLE_EQ(path.right_bound.front().y, 0.66); + EXPECT_DOUBLE_EQ(path.right_bound.front().z, 0.77); + } else { + FAIL() << "Yaml file might've corrupted."; + } } } // namespace autoware::test_utils diff --git a/common/autoware_time_utils/CHANGELOG.rst b/common/autoware_time_utils/CHANGELOG.rst index 73c72991895b0..43074de077e04 100644 --- a/common/autoware_time_utils/CHANGELOG.rst +++ b/common/autoware_time_utils/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package time_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_time_utils +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.38.0 (2024-11-08) ------------------- diff --git a/common/autoware_traffic_light_recognition_marker_publisher/CHANGELOG.rst b/common/autoware_traffic_light_recognition_marker_publisher/CHANGELOG.rst index d4f17b0feea0e..98691121f4d30 100644 --- a/common/autoware_traffic_light_recognition_marker_publisher/CHANGELOG.rst +++ b/common/autoware_traffic_light_recognition_marker_publisher/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package traffic_light_recognition_marker_publisher -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_traffic_light_recognition_marker_publisher +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.38.0 (2024-11-08) ------------------- diff --git a/common/autoware_traffic_light_recognition_marker_publisher/CMakeLists.txt b/common/autoware_traffic_light_recognition_marker_publisher/CMakeLists.txt index 281a6fc7dc802..1babe2e464fec 100644 --- a/common/autoware_traffic_light_recognition_marker_publisher/CMakeLists.txt +++ b/common/autoware_traffic_light_recognition_marker_publisher/CMakeLists.txt @@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "TrafficLightRecognitionMarkerPublisher" + PLUGIN "autoware::traffic_light_recognition_marker_publisher::TrafficLightRecognitionMarkerPublisher" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/common/autoware_traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp b/common/autoware_traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp index 5d887a2296137..a08145f429c72 100644 --- a/common/autoware_traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp +++ b/common/autoware_traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp @@ -17,9 +17,12 @@ #include #include #include +#include #include #include +namespace autoware::traffic_light_recognition_marker_publisher +{ TrafficLightRecognitionMarkerPublisher::TrafficLightRecognitionMarkerPublisher( const rclcpp::NodeOptions & options) : Node("traffic_light_recognition_marker_publisher", options) @@ -159,6 +162,8 @@ std_msgs::msg::ColorRGBA TrafficLightRecognitionMarkerPublisher::getTrafficLight return c; // white } +} // namespace autoware::traffic_light_recognition_marker_publisher #include -RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightRecognitionMarkerPublisher) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::traffic_light_recognition_marker_publisher::TrafficLightRecognitionMarkerPublisher) diff --git a/common/autoware_traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp b/common/autoware_traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp index 14bb0e39628f9..b4fa388fe0167 100644 --- a/common/autoware_traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp +++ b/common/autoware_traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp @@ -29,6 +29,8 @@ #include #include +namespace autoware::traffic_light_recognition_marker_publisher +{ class TrafficLightRecognitionMarkerPublisher : public rclcpp::Node { public: @@ -58,5 +60,6 @@ class TrafficLightRecognitionMarkerPublisher : public rclcpp::Node bool is_map_ready_ = false; int32_t marker_id = 0; }; +} // namespace autoware::traffic_light_recognition_marker_publisher #endif // TRAFFIC_LIGHT_RECOGNITION_MARKER_PUBLISHER_HPP_ diff --git a/common/autoware_traffic_light_utils/CHANGELOG.rst b/common/autoware_traffic_light_utils/CHANGELOG.rst index 7a62fa661e9bb..d74774ae8ef3a 100644 --- a/common/autoware_traffic_light_utils/CHANGELOG.rst +++ b/common/autoware_traffic_light_utils/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package traffic_light_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_traffic_light_utils +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.38.0 (2024-11-08) ------------------- diff --git a/common/autoware_traffic_light_utils/src/traffic_light_utils.cpp b/common/autoware_traffic_light_utils/src/traffic_light_utils.cpp index 4ba7e1c1d57d8..97d3081ceefdd 100644 --- a/common/autoware_traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/autoware_traffic_light_utils/src/traffic_light_utils.cpp @@ -14,6 +14,8 @@ #include "autoware/traffic_light_utils/traffic_light_utils.hpp" +#include + namespace autoware::traffic_light_utils { diff --git a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp b/common/autoware_trajectory/examples/example_trajectory_path_point.cpp index 777942654e5b2..fb1197e53ab67 100644 --- a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_path_point.cpp @@ -19,6 +19,7 @@ #include +#include #include autoware_planning_msgs::msg::PathPoint path_point(double x, double y) diff --git a/common/autoware_trajectory/include/autoware/trajectory/point.hpp b/common/autoware_trajectory/include/autoware/trajectory/point.hpp index e9d985a85194a..3e763e2428f1b 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/point.hpp @@ -122,7 +122,7 @@ class Trajectory using trajectory::detail::to_point; Eigen::Vector2d point(to_point(p).x, to_point(p).y); std::vector distances_from_segments; - std::vector lengthes_from_start_points; + std::vector lengths_from_start_points; auto axis = detail::crop_bases(bases_, start_, end_); @@ -149,7 +149,7 @@ class Trajectory } if (constraints(length_from_start_point)) { distances_from_segments.push_back(distance_from_segment); - lengthes_from_start_points.push_back(length_from_start_point); + lengths_from_start_points.push_back(length_from_start_point); } } if (distances_from_segments.empty()) { @@ -158,7 +158,7 @@ class Trajectory auto min_it = std::min_element(distances_from_segments.begin(), distances_from_segments.end()); - return lengthes_from_start_points[std::distance(distances_from_segments.begin(), min_it)] - + return lengths_from_start_points[std::distance(distances_from_segments.begin(), min_it)] - start_; } diff --git a/common/autoware_trajectory/src/path_point.cpp b/common/autoware_trajectory/src/path_point.cpp index af1e9286b533d..0806bcead53f3 100644 --- a/common/autoware_trajectory/src/path_point.cpp +++ b/common/autoware_trajectory/src/path_point.cpp @@ -18,6 +18,8 @@ #include +#include + namespace autoware::trajectory { @@ -76,7 +78,7 @@ std::vector Trajectory::restore(const size_t & min_points) points.reserve(bases.size()); for (const auto & s : bases) { PointType p; - p.pose = Trajectory::compute(s); + p.pose = Trajectory::compute(s - start_); p.longitudinal_velocity_mps = static_cast(this->longitudinal_velocity_mps.compute(s)); p.lateral_velocity_mps = static_cast(this->lateral_velocity_mps.compute(s)); p.heading_rate_rps = static_cast(this->heading_rate_rps.compute(s)); diff --git a/common/autoware_trajectory/src/path_point_with_lane_id.cpp b/common/autoware_trajectory/src/path_point_with_lane_id.cpp index 6c983c4dfa0d3..c3d7441fef405 100644 --- a/common/autoware_trajectory/src/path_point_with_lane_id.cpp +++ b/common/autoware_trajectory/src/path_point_with_lane_id.cpp @@ -65,7 +65,7 @@ std::vector Trajectory::restore(const size_t & min_points) points.reserve(bases.size()); for (const auto & s : bases) { PointType p; - p.point = BaseClass::compute(s); + p.point = BaseClass::compute(s - start_); p.lane_ids = lane_ids.compute(s); points.emplace_back(p); } diff --git a/common/autoware_trajectory/src/point.cpp b/common/autoware_trajectory/src/point.cpp index c7b9a6ab74a38..40c6c357e9ba5 100644 --- a/common/autoware_trajectory/src/point.cpp +++ b/common/autoware_trajectory/src/point.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace autoware::trajectory { diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp index 35eca81981f89..3906ee17fa2eb 100644 --- a/common/autoware_trajectory/src/pose.cpp +++ b/common/autoware_trajectory/src/pose.cpp @@ -92,7 +92,7 @@ std::vector Trajectory::restore(const size_t & min_points) points.reserve(bases.size()); for (const auto & s : bases) { PointType p; - p.position = BaseClass::compute(s); + p.position = BaseClass::compute(s - start_); p.orientation = orientation_interpolator_->compute(s); points.emplace_back(p); } diff --git a/common/autoware_trajectory/test/test_trajectory_container.cpp b/common/autoware_trajectory/test/test_trajectory_container.cpp index b3e4c16a09999..6f6d52107b348 100644 --- a/common/autoware_trajectory/test/test_trajectory_container.cpp +++ b/common/autoware_trajectory/test/test_trajectory_container.cpp @@ -15,6 +15,7 @@ #include +#include #include using Trajectory = autoware::trajectory::Trajectory; @@ -106,9 +107,9 @@ TEST_F(TrajectoryTest, direction) TEST_F(TrajectoryTest, curvature) { - double curv = trajectory->curvature(0.0); - EXPECT_LT(-1.0, curv); - EXPECT_LT(curv, 1.0); + double curvature_val = trajectory->curvature(0.0); + EXPECT_LT(-1.0, curvature_val); + EXPECT_LT(curvature_val, 1.0); } TEST_F(TrajectoryTest, restore) diff --git a/common/autoware_universe_utils/examples/example_lru_cache.cpp b/common/autoware_universe_utils/examples/example_lru_cache.cpp index 95decf8eb336d..6ddf5ec165529 100644 --- a/common/autoware_universe_utils/examples/example_lru_cache.cpp +++ b/common/autoware_universe_utils/examples/example_lru_cache.cpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include using autoware::universe_utils::LRUCache; diff --git a/common/autoware_universe_utils/examples/example_polling_subscriber.cpp b/common/autoware_universe_utils/examples/example_polling_subscriber.cpp index d078da6df43af..b9a76356e07c4 100644 --- a/common/autoware_universe_utils/examples/example_polling_subscriber.cpp +++ b/common/autoware_universe_utils/examples/example_polling_subscriber.cpp @@ -18,6 +18,8 @@ #include +#include + // PublisherNode class definition class PublisherNode : public rclcpp::Node { diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/stat.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp similarity index 69% rename from evaluator/kinematic_evaluator/include/kinematic_evaluator/stat.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp index 096cb90b3dcf4..eb460cc685891 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/stat.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021-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. @@ -15,17 +15,17 @@ #include #include -#ifndef KINEMATIC_EVALUATOR__STAT_HPP_ -#define KINEMATIC_EVALUATOR__STAT_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__ACCUMULATOR_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__ACCUMULATOR_HPP_ -namespace kinematic_diagnostics +namespace autoware::universe_utils { /** - * @brief class to incrementally build statistics + * @brief class to accumulate statistical data, supporting min, max and mean. * @typedef T type of the values (default to double) */ template -class Stat +class Accumulator { public: /** @@ -65,11 +65,11 @@ class Stat unsigned int count() const { return count_; } template - friend std::ostream & operator<<(std::ostream & os, const Stat & stat); + friend std::ostream & operator<<(std::ostream & os, const Accumulator & accumulator); private: T min_ = std::numeric_limits::max(); - T max_ = std::numeric_limits::min(); + T max_ = std::numeric_limits::lowest(); long double mean_ = 0.0; unsigned int count_ = 0; }; @@ -78,16 +78,16 @@ class Stat * @brief overload << operator for easy print to output stream */ template -std::ostream & operator<<(std::ostream & os, const Stat & stat) +std::ostream & operator<<(std::ostream & os, const Accumulator & accumulator) { - if (stat.count() == 0) { + if (accumulator.count() == 0) { os << "None None None"; } else { - os << stat.min() << " " << stat.max() << " " << stat.mean(); + os << accumulator.min() << " " << accumulator.max() << " " << accumulator.mean(); } return os; } -} // namespace kinematic_diagnostics +} // namespace autoware::universe_utils -#endif // KINEMATIC_EVALUATOR__STAT_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__ACCUMULATOR_HPP_ diff --git a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp index 0d6e608d8cd6d..848ecdd699ec5 100644 --- a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp @@ -14,6 +14,11 @@ #include "autoware/universe_utils/geometry/alt_geometry.hpp" +#include +#include +#include +#include + namespace autoware::universe_utils { // Alternatives for Boost.Geometry ---------------------------------------------------------------- diff --git a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp index edc17b2dab120..f178a7e832511 100644 --- a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp +++ b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp @@ -14,6 +14,10 @@ #include "autoware/universe_utils/geometry/ear_clipping.hpp" +#include +#include +#include + namespace autoware::universe_utils { diff --git a/common/autoware_universe_utils/src/geometry/geometry.cpp b/common/autoware_universe_utils/src/geometry/geometry.cpp index 5fda8eb3f4ca4..e9aaccd055dad 100644 --- a/common/autoware_universe_utils/src/geometry/geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/geometry.cpp @@ -22,6 +22,8 @@ #include +#include + namespace tf2 { void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped & out) diff --git a/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp b/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp index aa3c2afab321a..35819c0b119bf 100644 --- a/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp +++ b/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp @@ -22,7 +22,12 @@ #include #include +#include +#include +#include #include +#include +#include namespace autoware::universe_utils { diff --git a/common/autoware_universe_utils/src/geometry/sat_2d.cpp b/common/autoware_universe_utils/src/geometry/sat_2d.cpp index eae1de39bb589..a38fcbd3c3430 100644 --- a/common/autoware_universe_utils/src/geometry/sat_2d.cpp +++ b/common/autoware_universe_utils/src/geometry/sat_2d.cpp @@ -14,6 +14,8 @@ #include "autoware/universe_utils/geometry/sat_2d.hpp" +#include + namespace autoware::universe_utils::sat { diff --git a/common/autoware_universe_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp index 2966f35bfe59e..5cfe1326323e5 100644 --- a/common/autoware_universe_utils/src/math/trigonometry.cpp +++ b/common/autoware_universe_utils/src/math/trigonometry.cpp @@ -18,6 +18,7 @@ #include "autoware/universe_utils/math/sin_table.hpp" #include +#include namespace autoware::universe_utils { diff --git a/common/autoware_universe_utils/src/ros/marker_helper.cpp b/common/autoware_universe_utils/src/ros/marker_helper.cpp index 0d360e2335df7..3370f7e05814a 100644 --- a/common/autoware_universe_utils/src/ros/marker_helper.cpp +++ b/common/autoware_universe_utils/src/ros/marker_helper.cpp @@ -14,6 +14,8 @@ #include "autoware/universe_utils/ros/marker_helper.hpp" +#include + namespace autoware::universe_utils { visualization_msgs::msg::Marker createDefaultMarker( diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index 8c715c5ccd7e3..55901ce5b4987 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -19,7 +19,10 @@ #include +#include #include +#include +#include namespace autoware::universe_utils { @@ -135,9 +138,10 @@ void TimeKeeper::start_track(const std::string & func_name) root_node_thread_id_ = std::this_thread::get_id(); } else { if (root_node_thread_id_ != std::this_thread::get_id()) { - RCLCPP_WARN( - rclcpp::get_logger("TimeKeeper"), - "TimeKeeper::start_track() is called from a different thread. Ignoring the call."); + const auto warning_msg = fmt::format( + "TimeKeeper::start_track({}) is called from a different thread. Ignoring the call.", + func_name); + RCLCPP_WARN(rclcpp::get_logger("TimeKeeper"), "%s", warning_msg.c_str()); return; } current_time_node_ = current_time_node_->add_child(func_name); diff --git a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp index 90dfb1ede4701..9fa949cedb867 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp @@ -25,7 +25,10 @@ #include #include +#include +#include #include +#include constexpr double epsilon = 1e-6; diff --git a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp index 3202347cc8ac4..3e4b08c4cc272 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -29,7 +29,10 @@ #include #include +#include +#include #include +#include constexpr double epsilon = 1e-6; diff --git a/common/autoware_universe_utils/test/src/math/test_accumulator.cpp b/common/autoware_universe_utils/test/src/math/test_accumulator.cpp new file mode 100644 index 0000000000000..89630454d5f19 --- /dev/null +++ b/common/autoware_universe_utils/test/src/math/test_accumulator.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/universe_utils/math/accumulator.hpp" + +#include + +TEST(accumulator, empty) +{ + autoware::universe_utils::Accumulator acc; + + EXPECT_DOUBLE_EQ(acc.mean(), 0.0); + EXPECT_DOUBLE_EQ(acc.min(), std::numeric_limits::max()); + EXPECT_DOUBLE_EQ(acc.max(), std::numeric_limits::lowest()); + EXPECT_EQ(acc.count(), 0); +} + +TEST(accumulator, addValues) +{ + autoware::universe_utils::Accumulator acc; + acc.add(100.0); + + EXPECT_DOUBLE_EQ(acc.mean(), 100.0); + EXPECT_DOUBLE_EQ(acc.min(), 100.0); + EXPECT_DOUBLE_EQ(acc.max(), 100.0); + EXPECT_EQ(acc.count(), 1); +} + +TEST(accumulator, positiveValues) +{ + autoware::universe_utils::Accumulator acc; + acc.add(10.0); + acc.add(40.0); + acc.add(10.0); + + EXPECT_DOUBLE_EQ(acc.mean(), 20.0); + EXPECT_DOUBLE_EQ(acc.min(), 10.0); + EXPECT_DOUBLE_EQ(acc.max(), 40.0); + EXPECT_EQ(acc.count(), 3); +} + +TEST(accumulator, negativeValues) +{ + autoware::universe_utils::Accumulator acc; + acc.add(-10.0); + acc.add(-40.0); + acc.add(-10.0); + + EXPECT_DOUBLE_EQ(acc.mean(), -20.0); + EXPECT_DOUBLE_EQ(acc.min(), -40.0); + EXPECT_DOUBLE_EQ(acc.max(), -10.0); + EXPECT_EQ(acc.count(), 3); +} diff --git a/common/autoware_universe_utils/test/src/ros/test_managed_transform_buffer.cpp b/common/autoware_universe_utils/test/src/ros/test_managed_transform_buffer.cpp index 9df6f9bb2ffef..351cd252126cc 100644 --- a/common/autoware_universe_utils/test/src/ros/test_managed_transform_buffer.cpp +++ b/common/autoware_universe_utils/test/src/ros/test_managed_transform_buffer.cpp @@ -21,6 +21,7 @@ #include #include +#include class TestManagedTransformBuffer : public ::testing::Test { diff --git a/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp b/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp index f9c0bd45c4fb4..7e4debfe59c2b 100644 --- a/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp +++ b/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp @@ -20,6 +20,10 @@ #include +#include +#include +#include + class PublishedTimePublisherWithSubscriptionTest : public ::testing::Test { protected: diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp index 0540fa8a283ad..4491d79051059 100644 --- a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -19,7 +19,9 @@ #include #include +#include #include +#include #include class TimeKeeperTest : public ::testing::Test @@ -86,7 +88,11 @@ TEST_F(TimeKeeperTest, MultiThreadWarning) t.join(); std::string err = testing::internal::GetCapturedStderr(); - EXPECT_TRUE( - err.find("TimeKeeper::start_track() is called from a different thread. Ignoring the call.") != - std::string::npos); + const bool error_found = err.find( + "TimeKeeper::start_track(MainFunction) is called from a different " + "thread. Ignoring the call.") != std::string::npos || + err.find( + "TimeKeeper::start_track(ThreadFunction) is called from a different " + "thread. Ignoring the call.") != std::string::npos; + EXPECT_TRUE(error_found); } diff --git a/common/autoware_vehicle_info_utils/src/vehicle_info.cpp b/common/autoware_vehicle_info_utils/src/vehicle_info.cpp index c1e762811ede3..c0a4c240ba44a 100644 --- a/common/autoware_vehicle_info_utils/src/vehicle_info.cpp +++ b/common/autoware_vehicle_info_utils/src/vehicle_info.cpp @@ -16,6 +16,8 @@ #include +#include + namespace autoware::vehicle_info_utils { autoware::universe_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const diff --git a/common/autoware_vehicle_info_utils/test/test_vehicle_info_utils.cpp b/common/autoware_vehicle_info_utils/test/test_vehicle_info_utils.cpp index 3ad19ba9cab35..ea660208da2be 100644 --- a/common/autoware_vehicle_info_utils/test/test_vehicle_info_utils.cpp +++ b/common/autoware_vehicle_info_utils/test/test_vehicle_info_utils.cpp @@ -17,6 +17,8 @@ #include +#include + class VehicleInfoUtilTest : public ::testing::Test { protected: diff --git a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp index 79da89bd9e981..cae9660b51266 100644 --- a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp +++ b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace rviz_plugins { BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent) @@ -82,7 +84,7 @@ void BagTimeManagerPanel::onPauseClicked() pause_button_->setStyleSheet("background-color: #00FF00;"); auto req = std::make_shared(); client_resume_->async_send_request( - req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + req, []([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); } else { // do pause current_state_ = STATE::PAUSE; @@ -91,7 +93,7 @@ void BagTimeManagerPanel::onPauseClicked() pause_button_->setStyleSheet("background-color: #FF0000;"); auto req = std::make_shared(); client_pause_->async_send_request( - req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + req, []([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); } } diff --git a/common/glog_component/CMakeLists.txt b/common/glog_component/CMakeLists.txt deleted file mode 100644 index a233914cc1524..0000000000000 --- a/common/glog_component/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(glog_component) - -find_package(autoware_cmake REQUIRED) -autoware_package() - - -ament_auto_add_library(glog_component SHARED - src/glog_component.cpp -) -target_link_libraries(glog_component glog::glog) - -rclcpp_components_register_node(glog_component - PLUGIN "GlogComponent" - EXECUTABLE glog_component_node -) - -ament_auto_package() diff --git a/common/tier4_adapi_rviz_plugin/src/state_panel.cpp b/common/tier4_adapi_rviz_plugin/src/state_panel.cpp index 6d2980a7f2c4a..0525f5fdc1a09 100644 --- a/common/tier4_adapi_rviz_plugin/src/state_panel.cpp +++ b/common/tier4_adapi_rviz_plugin/src/state_panel.cpp @@ -280,7 +280,7 @@ void StatePanel::onInitialize() void StatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) { - auto changeButtonState = [this]( + auto changeButtonState = []( QPushButton * button, const bool is_desired_mode_available, const uint8_t current_mode = OperationModeState::UNKNOWN, const uint8_t desired_mode = OperationModeState::STOP) { diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp index b6637a3c619a5..a961d7c9250ce 100644 --- a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp @@ -62,9 +62,6 @@ static const Ogre::Quaternion ROBOT_TO_CAMERA_ROTATION = Ogre::Quaternion(Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Y) * Ogre::Quaternion(Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Z); -static const float PITCH_LIMIT_LOW = -Ogre::Math::HALF_PI + 0.001; -static const float PITCH_LIMIT_HIGH = Ogre::Math::HALF_PI - 0.001; - BirdEyeViewController::BirdEyeViewController() : dragging_(false) { scale_property_ = new rviz_common::properties::FloatProperty( diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp index 97216f238d593..b7d754b02d4bd 100644 --- a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp @@ -68,7 +68,6 @@ static const float PITCH_START = Ogre::Math::HALF_PI / 3; static const float YAW_START = Ogre::Math::PI; static const float DISTANCE_START = 30; static const float FOCAL_SHAPE_SIZE_START = 0.05; -static const bool FOCAL_SHAPE_FIXED_SIZE = true; static const char * TARGET_FRAME_START = "base_link"; // move camera up so the focal point appears in the lower image half diff --git a/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp index d427f63628d69..bcf9d774200e6 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp @@ -22,6 +22,8 @@ #include #include +#include + namespace rviz_plugins { PoseHistory::PoseHistory() : last_stamp_(0, 0, RCL_ROS_TIME) diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp index 391ebdc0c1c77..a54856122ffc9 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp @@ -22,6 +22,8 @@ #include #include +#include + #define EIGEN_MPL2_ONLY #include #include diff --git a/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp b/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp index 42f36ed6f4c08..276206075b6eb 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp @@ -27,6 +27,8 @@ #include +#include + namespace rviz_plugins { PoseWithCovarianceHistory::PoseWithCovarianceHistory() : last_stamp_(0, 0, RCL_ROS_TIME) diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 342e72d66c5f9..869f0c4d91793 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -16,6 +16,10 @@ #include +#include +#include +#include + namespace rviz_plugins { AutowarePathWithLaneIdDisplay::AutowarePathWithLaneIdDisplay() diff --git a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp index b525608a65625..ddb55a253b1cf 100644 --- a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp @@ -20,6 +20,9 @@ #include #include +#include +#include + namespace { std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index aa7f5fe41c295..14ad8a22a52e2 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -14,6 +14,7 @@ ament_index_cpp autoware_adapi_v1_msgs + autoware_motion_utils autoware_vehicle_msgs libqt5-core libqt5-gui diff --git a/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp index 70f35ed3793ab..81677a69eb66f 100644 --- a/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp @@ -17,11 +17,10 @@ #ifndef VELOCITY_STEERING_FACTORS_PANEL_HPP_ #define VELOCITY_STEERING_FACTORS_PANEL_HPP_ +#include #include #include #include -#include -#include #include #include #include @@ -29,6 +28,8 @@ #include #include #include +#include +#include #include @@ -49,6 +50,11 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel void onInitialize() override; protected: + static constexpr double JERK_DEFAULT = 1.0; + static constexpr double DECEL_LIMIT_DEFAULT = 1.0; + + static constexpr QColor COLOR_FREAK_PINK = {255, 0, 108}; + // Layout QGroupBox * makeVelocityFactorsGroup(); QGroupBox * makeSteeringFactorsGroup(); @@ -58,7 +64,14 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel // Planning QTableWidget * velocity_factors_table_{nullptr}; QTableWidget * steering_factors_table_{nullptr}; + QDoubleSpinBox * jerk_input_{nullptr}; + QDoubleSpinBox * decel_limit_input_{nullptr}; + + nav_msgs::msg::Odometry::ConstSharedPtr kinematic_state_; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr acceleration_; + rclcpp::Subscription::SharedPtr sub_kinematic_state_; + rclcpp::Subscription::SharedPtr sub_acceleration_; rclcpp::Subscription::SharedPtr sub_velocity_factors_; rclcpp::Subscription::SharedPtr sub_steering_factors_; diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index b7ea26e2c6895..78ddfae57cd0c 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp @@ -21,8 +21,10 @@ #include #include #include +#include #include +#include #include #include @@ -46,7 +48,8 @@ QGroupBox * VelocitySteeringFactorsPanel::makeVelocityFactorsGroup() auto vertical_header = new QHeaderView(Qt::Vertical); vertical_header->hide(); auto horizontal_header = new QHeaderView(Qt::Horizontal); - horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + horizontal_header->setSectionResizeMode(QHeaderView::ResizeToContents); + horizontal_header->setStretchLastSection(true); auto header_labels = QStringList({"Type", "Status", "Distance [m]", "Detail"}); velocity_factors_table_ = new QTableWidget(); @@ -54,7 +57,27 @@ QGroupBox * VelocitySteeringFactorsPanel::makeVelocityFactorsGroup() velocity_factors_table_->setHorizontalHeaderLabels(header_labels); velocity_factors_table_->setVerticalHeader(vertical_header); velocity_factors_table_->setHorizontalHeader(horizontal_header); - grid->addWidget(velocity_factors_table_, 0, 0); + grid->addWidget(velocity_factors_table_, 0, 0, 4, 1); + + auto * jerk_label = new QLabel("Jerk"); + grid->addWidget(jerk_label, 0, 1); + + jerk_input_ = new QDoubleSpinBox; + jerk_input_->setMinimum(0.0); + jerk_input_->setValue(JERK_DEFAULT); + jerk_input_->setSingleStep(0.1); + jerk_input_->setSuffix(" [m/s\u00B3]"); + grid->addWidget(jerk_input_, 1, 1); + + auto * decel_limit_label = new QLabel("Decel limit"); + grid->addWidget(decel_limit_label, 2, 1); + + decel_limit_input_ = new QDoubleSpinBox; + decel_limit_input_->setMinimum(0.0); + decel_limit_input_->setValue(DECEL_LIMIT_DEFAULT); + decel_limit_input_->setSingleStep(0.1); + decel_limit_input_->setSuffix(" [m/s\u00B2]"); + grid->addWidget(decel_limit_input_, 3, 1); group->setLayout(grid); return group; @@ -90,6 +113,17 @@ void VelocitySteeringFactorsPanel::onInitialize() raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); // Planning + sub_kinematic_state_ = raw_node_->create_subscription( + "/localization/kinematic_state", 10, + [this](const nav_msgs::msg::Odometry::ConstSharedPtr msg) { kinematic_state_ = msg; }); + + sub_acceleration_ = + raw_node_->create_subscription( + "/localization/acceleration", 10, + [this](const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) { + acceleration_ = msg; + }); + sub_velocity_factors_ = raw_node_->create_subscription( "/api/planning/velocity_factors", 10, std::bind(&VelocitySteeringFactorsPanel::onVelocityFactors, this, _1)); @@ -155,6 +189,26 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray:: label->setAlignment(Qt::AlignCenter); velocity_factors_table_->setCellWidget(i, 3, label); } + + const auto row_background = [&]() -> QBrush { + if (!kinematic_state_ || !acceleration_) { + return {}; + } + const auto & current_vel = kinematic_state_->twist.twist.linear.x; + const auto & current_acc = acceleration_->accel.accel.linear.x; + const auto acc_min = -decel_limit_input_->value(); + const auto jerk_acc = jerk_input_->value(); + const auto decel_dist = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( + current_vel, 0., current_acc, acc_min, jerk_acc, -jerk_acc); + if (decel_dist > e.distance && e.distance >= 0 && e.status == VelocityFactor::APPROACHING) { + return COLOR_FREAK_PINK; + } + return {}; + }(); + for (int j = 0; j < velocity_factors_table_->columnCount(); j++) { + velocity_factors_table_->setItem(i, j, new QTableWidgetItem); + velocity_factors_table_->item(i, j)->setBackground(row_background); + } } velocity_factors_table_->update(); } diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index f0f4a6c9f7014..7133e4ea6b7f7 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -60,7 +60,7 @@ We do not activate AEB module if it satisfies the following conditions. ### 2. Generate a predicted path of the ego vehicle -#### IMU path generation +#### 2.1 Overview of IMU Path Generation AEB generates a predicted footprint path based on current velocity and current angular velocity obtained from attached sensors. Note that if `use_imu_path` is `false`, it skips this step. This predicted path is generated as: @@ -72,16 +72,81 @@ $$ where $v$ and $\omega$ are current longitudinal velocity and angular velocity respectively. $dt$ is time interval that users can define in advance with the `imu_prediction_time_interval` parameter. The IMU path is generated considering a time horizon, defined by the `imu_prediction_time_horizon` parameter. -Since the IMU path generation only uses the ego vehicle's current angular velocity, disregarding the MPC's planner steering, the shape of the IMU path tends to get distorted quite easily and protrude out of the ego vehicle's current lane, possibly causing unwanted emergency stops. That is why it is not recommended to use a big `imu_prediction_time_horizon`. To fix this issue, the IMU path's length can be cut short by tuning the `max_generated_imu_path_length` parameter, the IMU path generation will be cut short once its length surpasses this parameter's value. +#### 2.2 Constraints and Countermeasures in IMU Path Generation -It is also possible to limit the IMU path length based on the predicted lateral deviation of the vehicle by setting the `limit_imu_path_lat_dev` parameter flag to "true", and setting a threshold lateral deviation with the `imu_path_lat_dev_threshold` parameter. That way, the AEB will generate a predicted IMU path until a given predicted ego pose surpasses the lateral deviation threshold, and the IMU path will be cut short when the ego vehicle is turning relatively fast. +Since the IMU path generation only uses the ego vehicle's current angular velocity, disregarding the MPC's planner steering, the shape of the IMU path tends to get distorted quite easily and protrude out of the ego vehicle's current lane, possibly causing unwanted emergency stops. There are two countermeasures for this issue: -The advantage of setting a lateral deviation limit with the `limit_imu_path_lat_dev` parameter is that the `imu_prediction_time_horizon` and the `max_generated_imu_path_length` can be increased without worries about the IMU predicted path deforming beyond a certain threshold. The downside is that the IMU path will be cut short when the ego has a high angular velocity, in said cases, the AEB module would mostly rely on the MPC path to prevent or mitigate collisions. If it is assumed the ego vehicle will mostly travel along the centerline of its lanelets, it can be useful to set the lateral deviation threshold parameter `imu_path_lat_dev_threshold` to be equal to or smaller than the average lanelet width divided by 2, that way, the chance of the IMU predicted path leaving the current ego lanelet is smaller, and it is possible to increase the `imu_prediction_time_horizon` to prevent frontal collisions when the ego is mostly traveling in a straight line. +1. Control using the `max_generated_imu_path_length` parameter + + - Generation stops when path length exceeds the set value + - Avoid using a large `imu_prediction_time_horizon` + +2. Control based on lateral deviation + - Set the `limit_imu_path_lat_dev` parameter to "true" + - Set deviation threshold using `imu_path_lat_dev_threshold` + - Path generation stops when lateral deviation exceeds the threshold + +#### 2.3 Advantages and Limitations of Lateral Deviation Control + +The advantage of setting a lateral deviation limit with the `limit_imu_path_lat_dev` parameter is that the `imu_prediction_time_horizon` and the `max_generated_imu_path_length` can be increased without worries about the IMU predicted path deforming beyond a certain threshold. The downside is that the IMU path will be cut short when the ego has a high angular velocity, in said cases, the AEB module would mostly rely on the MPC path to prevent or mitigate collisions. + +If it is assumed the ego vehicle will mostly travel along the centerline of its lanelets, it can be useful to set the lateral deviation threshold parameter `imu_path_lat_dev_threshold` to be equal to or smaller than the average lanelet width divided by 2, that way, the chance of the IMU predicted path leaving the current ego lanelet is smaller, and it is possible to increase the `imu_prediction_time_horizon` to prevent frontal collisions when the ego is mostly traveling in a straight line. The lateral deviation is measured using the ego vehicle's current position as a reference, and it measures the distance of the furthermost vertex of the predicted ego footprint to the predicted path. The following image illustrates how the lateral deviation of a given ego pose is measured. ![measuring_lat_dev](./image/measuring-lat-dev-on-imu-path.drawio.svg) +#### 2.4 IMU Path Generation Algorithm + +##### 2.4.1 Selection of Lateral Deviation Check Points + +Select vehicle vertices for lateral deviation checks based on the following conditions: + +- Forward motion ($v > 0$) + - Right turn ($\omega > 0$): Right front vertex + - Left turn ($\omega < 0$): Left front vertex +- Reverse motion ($v < 0$) + - Right turn ($\omega > 0$): Right rear vertex + - Left turn ($\omega < 0$): Left rear vertex +- Straight motion ($\omega = 0$): Check both front/rear vertices depending on forward/reverse motion + +##### 2.4.2 Path Generation Process + +Execute the following steps at each time step: + +1. State Update + + - Calculate next position $(x_{k+1}, y_{k+1})$ and yaw angle $\theta_{k+1}$ based on current velocity $v$ and angular velocity $\omega$ + - Time interval $dt$ is based on the `imu_prediction_time_interval` parameter + +2. Vehicle Footprint Generation + + - Place vehicle footprint at calculated position + - Calculate check point coordinates + +3. Lateral Deviation Calculation + + - Calculate lateral deviation from selected vertex to path + - Update path length and elapsed time + +4. Evaluation of Termination Conditions + +##### 2.4.3 Termination Conditions + +Path generation terminates when any of the following conditions are met: + +1. Basic Termination Conditions (both must be satisfied) + + - Predicted time exceeds `imu_prediction_time_horizon` + - AND path length exceeds `min_generated_imu_path_length` + +2. Path Length Termination Condition + + - Path length exceeds `max_generated_imu_path_length` + +3. Lateral Deviation Termination Condition (when `limit_imu_path_lat_dev = true`) + - Lateral deviation of selected vertex exceeds `imu_path_lat_dev_threshold` + #### MPC path generation If the `use_predicted_trajectory` parameter is set to true, the AEB module will directly use the predicted path from the MPC as a base to generate a footprint path. It will copy the ego poses generated by the MPC until a given time horizon. The `mpc_prediction_time_horizon` parameter dictates how far ahead in the future the MPC path will predict the ego vehicle's movement. Both the IMU footprint path and the MPC footprint path can be used at the same time. @@ -243,7 +308,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | min_generated_imu_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | | max_generated_imu_path_length | [m] | double | maximum distance for a predicted path generated by sensors | 10.0 | | expand_width | [m] | double | expansion width of the ego vehicle for collision checking, path cropping and speed calculation | 0.1 | -| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | +| longitudinal_offset_margin | [m] | double | longitudinal offset distance for collision check | 2.0 | | t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | | a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | | a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 99ca4d4ef52cb..b23c3bbdd4d59 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -4,6 +4,7 @@ use_predicted_trajectory: true use_imu_path: true limit_imu_path_lat_dev: false + limit_imu_path_length: true use_pointcloud_data: true use_predicted_object_data: false use_object_velocity_calculation: true @@ -39,7 +40,7 @@ maximum_cluster_size: 10000 # RSS distance collision check - longitudinal_offset: 1.0 + longitudinal_offset_margin: 1.0 t_response: 1.0 a_ego_min: -3.0 a_obj_min: -1.0 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 96efb5dc9ff1f..3de8a52c70643 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -556,6 +556,7 @@ class AEB : public rclcpp::Node bool use_predicted_trajectory_; bool use_imu_path_; bool limit_imu_path_lat_dev_; + bool limit_imu_path_length_; bool use_pointcloud_data_; bool use_predicted_object_data_; bool use_object_velocity_calculation_; @@ -571,7 +572,7 @@ class AEB : public rclcpp::Node double min_generated_imu_path_length_; double max_generated_imu_path_length_; double expand_width_; - double longitudinal_offset_; + double longitudinal_offset_margin_; double t_response_; double a_ego_min_; double a_obj_min_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 18f3716b755ee..3b1994481c8b1 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -15,9 +15,12 @@ #ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ #define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ +#include "autoware/autonomous_emergency_braking/node.hpp" + #include #include +#include #include #include @@ -49,6 +52,26 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; +/** + * @brief Get the Object data of an obstacle inside the ego path + * @param path the ego path + * @param front_offset offset from ego baselink to front + * @param rear_offset offset from ego baselink to back + */ +std::optional getObjectOnPathData( + const std::vector & ego_path, const geometry_msgs::msg::Point & obj_position, + const rclcpp::Time & stamp, const double path_length, const double path_width, + const double path_expansion, const double longitudinal_offset, const double object_speed = 0.0); + +/** + * @brief Get the longitudinal offset based on vehicle traveling direction + * @param path the ego path + * @param front_offset offset from ego baselink to front + * @param rear_offset offset from ego baselink to back + */ +std::optional getLongitudinalOffset( + const std::vector & path, const double front_offset, const double rear_offset); + /** * @brief Apply a transform to a predicted object * @param input the predicted object diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index edec3ac8fd6a1..5fac12cb3e715 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -9,6 +9,7 @@ Mamoru Sobue Daniel Sanchez Kyoichi Sugahara + Alqudah Mohammad Apache License 2.0 diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 7c4cf58cbc6e7..eb0884e4f344e 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -43,10 +43,13 @@ #include #include +#include #include #include #include +#include #include +#include #include #ifdef ROS_DISTRO_GALACTIC #include @@ -57,6 +60,16 @@ #include #endif +namespace +{ +using autoware::motion::control::autonomous_emergency_braking::colorTuple; +constexpr double MIN_MOVING_VELOCITY_THRESHOLD = 0.1; +// Sky blue (RGB: 0, 148, 205) - A medium-bright blue color +constexpr colorTuple IMU_PATH_COLOR = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; +// Forest green (RGB: 0, 100, 0) - A deep, dark green color +constexpr colorTuple MPC_PATH_COLOR = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; +} // namespace + namespace autoware::motion::control::autonomous_emergency_braking { using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; @@ -156,6 +169,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); limit_imu_path_lat_dev_ = declare_parameter("limit_imu_path_lat_dev"); + limit_imu_path_length_ = declare_parameter("limit_imu_path_length"); use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); use_predicted_object_data_ = declare_parameter("use_predicted_object_data"); use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); @@ -173,7 +187,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) min_generated_imu_path_length_ = declare_parameter("min_generated_imu_path_length"); max_generated_imu_path_length_ = declare_parameter("max_generated_imu_path_length"); expand_width_ = declare_parameter("expand_width"); - longitudinal_offset_ = declare_parameter("longitudinal_offset"); + longitudinal_offset_margin_ = declare_parameter("longitudinal_offset_margin"); t_response_ = declare_parameter("t_response"); a_ego_min_ = declare_parameter("a_ego_min"); a_obj_min_ = declare_parameter("a_obj_min"); @@ -220,6 +234,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); updateParam(parameters, "limit_imu_path_lat_dev", limit_imu_path_lat_dev_); + updateParam(parameters, "limit_imu_path_length", limit_imu_path_length_); updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); updateParam(parameters, "use_predicted_object_data", use_predicted_object_data_); updateParam( @@ -238,7 +253,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_); updateParam(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_); updateParam(parameters, "expand_width", expand_width_); - updateParam(parameters, "longitudinal_offset", longitudinal_offset_); + updateParam(parameters, "longitudinal_offset_margin", longitudinal_offset_margin_); updateParam(parameters, "t_response", t_response_); updateParam(parameters, "a_ego_min", a_ego_min_); updateParam(parameters, "a_obj_min", a_obj_min_); @@ -460,9 +475,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers) } // step2. create velocity data check if the vehicle stops or not - constexpr double min_moving_velocity_th{0.1}; const double current_v = current_velocity_ptr_->longitudinal_velocity; - if (std::abs(current_v) < min_moving_velocity_th) { + if (std::abs(current_v) < MIN_MOVING_VELOCITY_THRESHOLD) { return false; } @@ -570,18 +584,16 @@ bool AEB::checkCollision(MarkerArray & debug_markers) getPointsBelongingToClusterHulls( filtered_objects, points_belonging_to_cluster_hulls, debug_markers); - const auto imu_path_objects = (!use_imu_path_ || !angular_velocity_ptr_) - ? std::vector{} - : get_objects_on_path( - ego_imu_path, points_belonging_to_cluster_hulls, - {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}, "imu"); + const auto imu_path_objects = + (!use_imu_path_ || !angular_velocity_ptr_) + ? std::vector{} + : get_objects_on_path(ego_imu_path, points_belonging_to_cluster_hulls, IMU_PATH_COLOR, "imu"); const auto mpc_path_objects = (!use_predicted_trajectory_ || !predicted_traj_ptr_ || !ego_mpc_path.has_value()) ? std::vector{} : get_objects_on_path( - ego_mpc_path.value(), points_belonging_to_cluster_hulls, - {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}, "mpc"); + ego_mpc_path.value(), points_belonging_to_cluster_hulls, MPC_PATH_COLOR, "mpc"); // merge object data which comes from the ego (imu) path and predicted path auto merge_objects = @@ -632,7 +644,7 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object const double obj_braking_distance = (obj_v > 0.0) ? -(obj_v * obj_v) / (2 * std::fabs(a_obj_min_)) : (obj_v * obj_v) / (2 * std::fabs(a_obj_min_)); - return ego_stopping_distance + obj_braking_distance + longitudinal_offset_; + return ego_stopping_distance + obj_braking_distance + longitudinal_offset_margin_; }); tier4_debug_msgs::msg::Float32Stamped rss_distance_msg; @@ -655,10 +667,9 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) const double & dt = imu_prediction_time_interval_; const double distance_between_points = std::abs(curr_v) * dt; constexpr double minimum_distance_between_points{1e-2}; - // if current velocity is too small, assume it stops at the same point // if distance between points is too small, arc length calculation is unreliable, so we skip // creating the path - if (std::abs(curr_v) < 0.1 || distance_between_points < minimum_distance_between_points) { + if (distance_between_points < minimum_distance_between_points) { return {}; } @@ -700,7 +711,8 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) const bool basic_path_conditions_satisfied = (t > horizon) && (path_arc_length > min_generated_imu_path_length_); - const bool path_length_threshold_surpassed = path_arc_length > max_generated_imu_path_length_; + const bool path_length_threshold_surpassed = + limit_imu_path_length_ && path_arc_length > max_generated_imu_path_length_; const bool lat_dev_threshold_surpassed = limit_imu_path_lat_dev_ && std::abs(edge_of_ego_vehicle.y) > imu_path_lat_dev_threshold_; @@ -815,6 +827,13 @@ void AEB::createObjectDataUsingPredictedObjects( const auto transform_stamped_opt = utils::getTransform("base_link", predicted_objects_ptr_->header.frame_id, tf_buffer_, logger); if (!transform_stamped_opt.has_value()) return; + + const auto longitudinal_offset_opt = utils::getLongitudinalOffset( + ego_path, vehicle_info_.max_longitudinal_offset_m, vehicle_info_.rear_overhang_m); + + if (!longitudinal_offset_opt.has_value()) return; + const auto longitudinal_offset = longitudinal_offset_opt.value(); + // Check which objects collide with the ego footprints std::for_each(objects.begin(), objects.end(), [&](const auto & predicted_object) { // get objects in base_link frame @@ -842,10 +861,7 @@ void AEB::createObjectDataUsingPredictedObjects( // If the object is behind the ego, we need to use the backward long offset. The // distance should be a positive number in any case - const bool is_object_in_front_of_ego = obj_arc_length > 0.0; - const double dist_ego_to_object = - (is_object_in_front_of_ego) ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m - : obj_arc_length + vehicle_info_.min_longitudinal_offset_m; + const double dist_ego_to_object = obj_arc_length - longitudinal_offset; ObjectData obj; obj.stamp = stamp; @@ -928,53 +944,21 @@ void AEB::getClosestObjectsOnPath( if (ego_path.size() < 2 || points_belonging_to_cluster_hulls->empty()) { return; } - const auto ego_is_driving_forward_opt = autoware::motion_utils::isDrivingForward(ego_path); - if (!ego_is_driving_forward_opt.has_value()) { - return; - } - const bool ego_is_driving_forward = ego_is_driving_forward_opt.value(); - // select points inside the ego footprint path - const auto current_p = [&]() { - const auto & first_point_of_path = ego_path.front(); - const auto & p = first_point_of_path.position; - return autoware::universe_utils::createPoint(p.x, p.y, p.z); - }(); + const auto longitudinal_offset_opt = utils::getLongitudinalOffset( + ego_path, vehicle_info_.max_longitudinal_offset_m, vehicle_info_.rear_overhang_m); + + if (!longitudinal_offset_opt.has_value()) return; + const auto longitudinal_offset = longitudinal_offset_opt.value(); const auto path_length = autoware::motion_utils::calcArcLength(ego_path); + const auto path_width = vehicle_info_.vehicle_width_m / 2.0 + expand_width_; + // select points inside the ego footprint path for (const auto & p : *points_belonging_to_cluster_hulls) { const auto obj_position = autoware::universe_utils::createPoint(p.x, p.y, p.z); - const double obj_arc_length = - autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); - if ( - std::isnan(obj_arc_length) || - obj_arc_length > path_length + vehicle_info_.max_longitudinal_offset_m) - continue; - - // calculate the lateral offset between the ego vehicle and the object - const double lateral_offset = - std::abs(autoware::motion_utils::calcLateralOffset(ego_path, obj_position)); - - if (std::isnan(lateral_offset)) continue; - - // object is outside region of interest - if ( - lateral_offset > - vehicle_info_.vehicle_width_m / 2.0 + expand_width_ + speed_calculation_expansion_margin_) { - continue; - } - - // If the object is behind the ego, we need to use the backward long offset. The distance should - // be a positive number in any case - const double dist_ego_to_object = (ego_is_driving_forward) - ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m - : obj_arc_length + vehicle_info_.min_longitudinal_offset_m; - ObjectData obj; - obj.stamp = stamp; - obj.position = obj_position; - obj.velocity = 0.0; - obj.distance_to_object = std::abs(dist_ego_to_object); - obj.is_target = (lateral_offset < vehicle_info_.vehicle_width_m / 2.0 + expand_width_); - objects.push_back(obj); + auto obj_data_opt = utils::getObjectOnPathData( + ego_path, obj_position, stamp, path_length, path_width, speed_calculation_expansion_margin_, + longitudinal_offset, 0.0); + if (obj_data_opt.has_value()) objects.push_back(obj_data_opt.value()); } } @@ -1101,9 +1085,9 @@ void AEB::addVirtualStopWallMarker(MarkerArray & markers) }); if (ego_map_pose.has_value()) { - const double base_to_front_offset = vehicle_info_.max_longitudinal_offset_m; + const double base_link_to_front_offset = vehicle_info_.max_longitudinal_offset_m; const auto ego_front_pose = autoware::universe_utils::calcOffsetPose( - ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0); + ego_map_pose.value(), base_link_to_front_offset, 0.0, 0.0, 0.0); const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker( ego_front_pose, "autonomous_emergency_braking", this->now(), 0); autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &markers); diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index 5d9782ada5fbd..abd203dee2a02 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -13,9 +13,12 @@ // limitations under the License. #include +#include #include #include +#include +#include namespace autoware::motion::control::autonomous_emergency_braking::utils { @@ -27,6 +30,55 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Vector3; +std::optional getObjectOnPathData( + const std::vector & ego_path, const geometry_msgs::msg::Point & obj_position, + const rclcpp::Time & stamp, const double path_length, const double path_width, + const double path_expansion, const double longitudinal_offset, const double object_speed) +{ + const auto current_p = [&]() { + const auto & p = ego_path.front().position; + return autoware::universe_utils::createPoint(p.x, p.y, p.z); + }(); + const double obj_arc_length = + autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); + if ( + std::isnan(obj_arc_length) || obj_arc_length < 0.0 || + obj_arc_length > path_length + longitudinal_offset) { + return {}; + } + + // calculate the lateral offset between the ego vehicle and the object + const double lateral_offset = + std::abs(autoware::motion_utils::calcLateralOffset(ego_path, obj_position)); + + // object is outside region of interest + if (std::isnan(lateral_offset) || lateral_offset > path_width + path_expansion) { + return {}; + } + + // If the object is behind the ego, we need to use the backward long offset. The distance should + // be a positive number in any case + const double dist_ego_to_object = obj_arc_length - longitudinal_offset; + ObjectData obj; + obj.stamp = stamp; + obj.position = obj_position; + obj.velocity = object_speed; + obj.distance_to_object = std::abs(dist_ego_to_object); + obj.is_target = (lateral_offset < path_width); + return obj; +} + +std::optional getLongitudinalOffset( + const std::vector & ego_path, const double front_offset, const double rear_offset) +{ + const auto ego_is_driving_forward_opt = autoware::motion_utils::isDrivingForward(ego_path); + if (!ego_is_driving_forward_opt.has_value()) { + return {}; + } + const bool ego_is_driving_forward = ego_is_driving_forward_opt.value(); + return (ego_is_driving_forward) ? front_offset : rear_offset; +} + PredictedObject transformObjectFrame( const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped) { diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 3c00a4e6b5caf..1845ab4074fad 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -21,14 +21,17 @@ #include #include +#include #include #include #include #include +#include #include #include +#include namespace autoware::motion::control::autonomous_emergency_braking::test { @@ -126,6 +129,76 @@ TEST_F(TestAEB, checkCollision) ASSERT_FALSE(aeb_node_->hasCollision(longitudinal_velocity, object_no_collision)); } +TEST_F(TestAEB, getObjectOnPathData) +{ + constexpr double longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.0; + const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate); + ASSERT_FALSE(imu_path.empty()); + + const double path_width = 2.0; + const auto path_length = autoware::motion_utils::calcArcLength(imu_path); + ASSERT_TRUE( + path_length < aeb_node_->max_generated_imu_path_length_ + + aeb_node_->imu_prediction_time_interval_ * longitudinal_velocity + + std::numeric_limits::epsilon()); + + const auto stamp = rclcpp::Time(); + + const auto longitudinal_offset_opt = utils::getLongitudinalOffset(imu_path, 1.0, -1.0); + ASSERT_TRUE(longitudinal_offset_opt.has_value()); + const auto longitudinal_offset = longitudinal_offset_opt.value(); + ASSERT_DOUBLE_EQ(longitudinal_offset, 1.0); + + // Object in path if longitudinal_offset is considered + Point obj_position; + double path_expansion; + + { + obj_position.x = path_length + std::numeric_limits::epsilon(); + obj_position.y = 1.0; + path_expansion = 0.0; + auto obj_data_opt = utils::getObjectOnPathData( + imu_path, obj_position, stamp, path_length, path_width, path_expansion, longitudinal_offset, + 0.0); + ASSERT_TRUE(obj_data_opt.has_value()); + ASSERT_TRUE(obj_data_opt.value().is_target); + } + + // Object outside of path + { + obj_position.x = path_length + std::numeric_limits::epsilon(); + obj_position.y = 3.0; + path_expansion = 0.0; + auto obj_data_opt = utils::getObjectOnPathData( + imu_path, obj_position, stamp, path_length, path_width, path_expansion, longitudinal_offset, + 0.0); + ASSERT_FALSE(obj_data_opt.has_value()); + } + + // Object outside of path + { + obj_position.x = -1.0; + obj_position.y = 0.0; + path_expansion = 0.0; + auto obj_data_opt = utils::getObjectOnPathData( + imu_path, obj_position, stamp, path_length, path_width, path_expansion, longitudinal_offset, + 0.0); + ASSERT_FALSE(obj_data_opt.has_value()); + } + + // Object is covered by path expansion + { + obj_position.x = path_length + std::numeric_limits::epsilon(); + obj_position.y = 3.0; + path_expansion = 2.0; + auto obj_data_opt = utils::getObjectOnPathData( + imu_path, obj_position, stamp, path_length, path_width, 2.0, longitudinal_offset, 0.0); + ASSERT_TRUE(obj_data_opt.has_value()); + ASSERT_FALSE(obj_data_opt.value().is_target); + } +} + TEST_F(TestAEB, checkImuPathGeneration) { constexpr double longitudinal_velocity = 3.0; @@ -145,7 +218,7 @@ TEST_F(TestAEB, checkImuPathGeneration) pcl::PointCloud::Ptr obstacle_points_ptr = pcl::make_shared>(); { - const double x_start{0.0}; + const double x_start{0.5}; const double y_start{0.0}; for (size_t i = 0; i < 15; ++i) { @@ -162,6 +235,7 @@ TEST_F(TestAEB, checkImuPathGeneration) MarkerArray debug_markers; aeb_node_->getPointsBelongingToClusterHulls( obstacle_points_ptr, points_belonging_to_cluster_hulls, debug_markers); + ASSERT_FALSE(points_belonging_to_cluster_hulls->empty()); std::vector objects; aeb_node_->getClosestObjectsOnPath(imu_path, stamp, points_belonging_to_cluster_hulls, objects); ASSERT_FALSE(objects.empty()); diff --git a/control/autoware_collision_detector/src/node.cpp b/control/autoware_collision_detector/src/node.cpp index cfb9e5bfc6dbb..77d6d76c08b0a 100644 --- a/control/autoware_collision_detector/src/node.cpp +++ b/control/autoware_collision_detector/src/node.cpp @@ -29,6 +29,12 @@ #include #include +#include +#include +#include +#include +#include + #define EIGEN_MPL2_ONLY #include #include diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index d51282cb33bb3..0c16fae065939 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace autoware::control_validator { diff --git a/control/autoware_control_validator/test/test_control_validator.cpp b/control/autoware_control_validator/test/test_control_validator.cpp index c4d5b04fa920c..76ba5b7894365 100644 --- a/control/autoware_control_validator/test/test_control_validator.cpp +++ b/control/autoware_control_validator/test/test_control_validator.cpp @@ -24,6 +24,7 @@ #include #include +#include #include using autoware_planning_msgs::msg::Trajectory; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 6c83f8780c93c..1ac984394a25e 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -157,19 +157,10 @@ class LaneDepartureChecker Param param_; std::shared_ptr vehicle_info_ptr_; - static PoseDeviation calcTrajectoryDeviation( - const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold); - - static std::vector createVehiclePassingAreas( - const std::vector & vehicle_footprints); - bool willLeaveLane( const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints) const; - double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const; - static SegmentRtree extractUncrossableBoundaries( const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point, const double max_search_length, const std::vector & boundary_types_to_detect); diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp index 45a651339cc12..bde404603749b 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ #include +#include #include #include @@ -23,11 +24,17 @@ #include #include +#include +#include +#include + #include namespace autoware::lane_departure_checker::utils { using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::PoseDeviation; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_planning_msgs::msg::PathWithLaneId; @@ -73,6 +80,54 @@ std::vector createVehicleFootprints( std::vector createVehicleFootprints( const PathWithLaneId & path, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double footprint_extra_margin); + +/** + * @brief find lanelets that potentially intersect with the vehicle's trajectory + * @param route_lanelets lanelets along the planned route + * @param vehicle_footprints series of vehicle footprint polygons along the trajectory + * @return lanelets that are not disjoint from the convex hull of vehicle footprints + */ +lanelet::ConstLanelets getCandidateLanelets( + const lanelet::ConstLanelets & route_lanelets, + const std::vector & vehicle_footprints); + +/** + * @brief create a convex hull from multiple footprint polygons + * @param footprints collection of footprint polygons represented as LinearRing2d + * @return a single LinearRing2d representing the convex hull containing all input footprints + */ +LinearRing2d createHullFromFootprints(const std::vector & footprints); + +/** + * @brief create passing areas of the vehicle from vehicle footprints + * @param vehicle_footprints vehicle footprints along trajectory + * @return passing areas of the vehicle that are created from adjacent vehicle footprints + * If vehicle_footprints is empty, returns empty vector + * If vehicle_footprints size is 1, returns vector with that footprint + */ +std::vector createVehiclePassingAreas( + const std::vector & vehicle_footprints); + +/** + * @brief calculate the deviation of the given pose from the nearest pose on the trajectory + * @param trajectory target trajectory + * @param pose vehicle pose + * @param dist_threshold distance threshold used for searching for first nearest index to given pose + * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose + * @return deviation of the given pose from the trajectory + */ +PoseDeviation calcTrajectoryDeviation( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold); + +/** + * @brief calculate the maximum search length for boundaries considering the vehicle dimensions + * @param trajectory target trajectory + * @param vehicle_info vehicle information + * @return maximum search length for boundaries + */ +double calcMaxSearchLengthForBoundaries( + const Trajectory & trajectory, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); } // namespace autoware::lane_departure_checker::utils #endif // AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index f823988c77e4d..5d50ac0e2951b 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -16,7 +16,6 @@ #include "autoware/lane_departure_checker/utils.hpp" -#include #include #include #include @@ -27,9 +26,9 @@ #include #include +#include #include -using autoware::motion_utils::calcArcLength; using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::LineString2d; using autoware::universe_utils::MultiPoint2d; @@ -60,39 +59,6 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 return false; } -LinearRing2d createHullFromFootprints(const std::vector & footprints) -{ - MultiPoint2d combined; - for (const auto & footprint : footprints) { - for (const auto & p : footprint) { - combined.push_back(p); - } - } - - LinearRing2d hull; - boost::geometry::convex_hull(combined, hull); - - return hull; -} - -lanelet::ConstLanelets getCandidateLanelets( - const lanelet::ConstLanelets & route_lanelets, - const std::vector & vehicle_footprints) -{ - lanelet::ConstLanelets candidate_lanelets; - - // Find lanes within the convex hull of footprints - const auto footprint_hull = createHullFromFootprints(vehicle_footprints); - for (const auto & route_lanelet : route_lanelets) { - const auto poly = route_lanelet.polygon2d().basicPolygon(); - if (!boost::geometry::disjoint(poly, footprint_hull)) { - candidate_lanelets.push_back(route_lanelet); - } - } - - return candidate_lanelets; -} - } // namespace namespace autoware::lane_departure_checker @@ -103,7 +69,7 @@ Output LaneDepartureChecker::update(const Input & input) autoware::universe_utils::StopWatch stop_watch; - output.trajectory_deviation = calcTrajectoryDeviation( + output.trajectory_deviation = utils::calcTrajectoryDeviation( *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold, param_.ego_nearest_yaw_threshold); output.processing_time_map["calcTrajectoryDeviation"] = stop_watch.toc(true); @@ -127,13 +93,13 @@ Output LaneDepartureChecker::update(const Input & input) param_.footprint_margin_scale); output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true); - output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints); + output.vehicle_passing_areas = utils::createVehiclePassingAreas(output.vehicle_footprints); output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true); const auto candidate_road_lanelets = - getCandidateLanelets(input.route_lanelets, output.vehicle_footprints); + utils::getCandidateLanelets(input.route_lanelets, output.vehicle_footprints); const auto candidate_shoulder_lanelets = - getCandidateLanelets(input.shoulder_lanelets, output.vehicle_footprints); + utils::getCandidateLanelets(input.shoulder_lanelets, output.vehicle_footprints); output.candidate_lanelets = candidate_road_lanelets; output.candidate_lanelets.insert( output.candidate_lanelets.end(), candidate_shoulder_lanelets.begin(), @@ -148,7 +114,7 @@ Output LaneDepartureChecker::update(const Input & input) output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); const double max_search_length_for_boundaries = - calcMaxSearchLengthForBoundaries(*input.predicted_trajectory); + utils::calcMaxSearchLengthForBoundaries(*input.predicted_trajectory, *vehicle_info_ptr_); const auto uncrossable_boundaries = extractUncrossableBoundaries( *input.lanelet_map, input.predicted_trajectory->points.front().pose.position, max_search_length_for_boundaries, input.boundary_types_to_detect); @@ -165,33 +131,11 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( std::vector vehicle_footprints = utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); - lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints); + lanelet::ConstLanelets candidate_lanelets = + utils::getCandidateLanelets(lanelets, vehicle_footprints); return willLeaveLane(candidate_lanelets, vehicle_footprints); } -PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( - const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, - const double yaw_threshold) -{ - const auto nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( - trajectory.points, pose, dist_threshold, yaw_threshold); - return autoware::universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); -} - -std::vector LaneDepartureChecker::createVehiclePassingAreas( - const std::vector & vehicle_footprints) -{ - // Create hull from two adjacent vehicle footprints - std::vector areas; - for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) { - const auto & footprint1 = vehicle_footprints.at(i); - const auto & footprint2 = vehicle_footprints.at(i + 1); - areas.push_back(createHullFromFootprints({footprint1, footprint2})); - } - - return areas; -} - bool LaneDepartureChecker::willLeaveLane( const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints) const @@ -215,7 +159,7 @@ std::vector> LaneDepartureChecker::getLanele // Get Footprint Hull basic polygon std::vector vehicle_footprints = utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); - LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints); + LinearRing2d footprint_hull = utils::createHullFromFootprints(vehicle_footprints); lanelet::BasicPolygon2d footprint_hull_basic_polygon = toBasicPolygon2D(footprint_hull); @@ -388,18 +332,6 @@ bool LaneDepartureChecker::isOutOfLane( return false; } -double LaneDepartureChecker::calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const -{ - const double max_ego_lon_length = std::max( - std::abs(vehicle_info_ptr_->max_longitudinal_offset_m), - std::abs(vehicle_info_ptr_->min_longitudinal_offset_m)); - const double max_ego_lat_length = std::max( - std::abs(vehicle_info_ptr_->max_lateral_offset_m), - std::abs(vehicle_info_ptr_->min_lateral_offset_m)); - const double max_ego_search_length = std::hypot(max_ego_lon_length, max_ego_lat_length); - return calcArcLength(trajectory.points) + max_ego_search_length; -} - SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries( const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point, const double max_search_length, const std::vector & boundary_types_to_detect) diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp index 324936c633158..ec0647115385d 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp @@ -14,10 +14,15 @@ #include "autoware/lane_departure_checker/utils.hpp" +#include #include #include +#include + +#include + namespace { struct FootprintMargin @@ -161,4 +166,82 @@ std::vector createVehicleFootprints( return vehicle_footprints; } + +lanelet::ConstLanelets getCandidateLanelets( + const lanelet::ConstLanelets & route_lanelets, + const std::vector & vehicle_footprints) +{ + lanelet::ConstLanelets candidate_lanelets; + + // Find lanes within the convex hull of footprints + const auto footprint_hull = createHullFromFootprints(vehicle_footprints); + + for (const auto & route_lanelet : route_lanelets) { + const auto poly = route_lanelet.polygon2d().basicPolygon(); + if (!boost::geometry::disjoint(poly, footprint_hull)) { + candidate_lanelets.push_back(route_lanelet); + } + } + + return candidate_lanelets; +} + +LinearRing2d createHullFromFootprints(const std::vector & footprints) +{ + MultiPoint2d combined; + for (const auto & footprint : footprints) { + for (const auto & p : footprint) { + combined.push_back(p); + } + } + + LinearRing2d hull; + boost::geometry::convex_hull(combined, hull); + + return hull; +} + +std::vector createVehiclePassingAreas( + const std::vector & vehicle_footprints) +{ + if (vehicle_footprints.empty()) { + return {}; + } + + if (vehicle_footprints.size() == 1) { + return {vehicle_footprints.front()}; + } + + std::vector areas; + areas.reserve(vehicle_footprints.size() - 1); + + for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) { + const auto & footprint1 = vehicle_footprints.at(i); + const auto & footprint2 = vehicle_footprints.at(i + 1); + areas.push_back(createHullFromFootprints({footprint1, footprint2})); + } + + return areas; +} + +PoseDeviation calcTrajectoryDeviation( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold) +{ + const auto nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + trajectory.points, pose, dist_threshold, yaw_threshold); + return autoware::universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); +} + +double calcMaxSearchLengthForBoundaries( + const Trajectory & trajectory, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + const double max_ego_lon_length = std::max( + std::abs(vehicle_info.max_longitudinal_offset_m), + std::abs(vehicle_info.min_longitudinal_offset_m)); + const double max_ego_lat_length = std::max( + std::abs(vehicle_info.max_lateral_offset_m), std::abs(vehicle_info.min_lateral_offset_m)); + const double max_ego_search_length = std::hypot(max_ego_lon_length, max_ego_lat_length); + return autoware::motion_utils::calcArcLength(trajectory.points) + max_ego_search_length; +} } // namespace autoware::lane_departure_checker::utils diff --git a/control/autoware_lane_departure_checker/test/test_calc_max_search_length_for_boundaries.cpp b/control/autoware_lane_departure_checker/test/test_calc_max_search_length_for_boundaries.cpp new file mode 100644 index 0000000000000..2a7cef58e381f --- /dev/null +++ b/control/autoware_lane_departure_checker/test/test_calc_max_search_length_for_boundaries.cpp @@ -0,0 +1,108 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/lane_departure_checker/utils.hpp" + +#include + +#include + +#include +#include + +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +namespace +{ +Trajectory create_trajectory(const std::vector & points) +{ + Trajectory trajectory; + for (const auto & point : points) { + TrajectoryPoint p; + p.pose.position.x = point.x(); + p.pose.position.y = point.y(); + trajectory.points.push_back(p); + } + return trajectory; +} + +// reference: +// https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml +constexpr double wheel_radius_m = 0.383; +constexpr double wheel_width_m = 0.235; +constexpr double wheel_base_m = 2.79; +constexpr double wheel_tread_m = 1.64; +constexpr double front_overhang_m = 1.0; +constexpr double rear_overhang_m = 1.1; +constexpr double left_overhang_m = 0.128; +constexpr double right_overhang_m = 0.128; +constexpr double vehicle_height_m = 2.5; +constexpr double max_steer_angle_rad = 0.70; +} // namespace + +struct CalcMaxSearchLengthForBoundariesParam +{ + std::string description; + std::vector trajectory_points; + double expected_max_search_length; +}; + +std::ostream & operator<<(std::ostream & os, const CalcMaxSearchLengthForBoundariesParam & p) +{ + return os << p.description; +} + +class CalcMaxSearchLengthForBoundariesTest +: public ::testing::TestWithParam +{ +protected: + void SetUp() override + { + vehicle_info = autoware::vehicle_info_utils::createVehicleInfo( + wheel_radius_m, wheel_width_m, wheel_base_m, wheel_tread_m, front_overhang_m, rear_overhang_m, + left_overhang_m, right_overhang_m, vehicle_height_m, max_steer_angle_rad); + } + + autoware::vehicle_info_utils::VehicleInfo vehicle_info; +}; + +TEST_P(CalcMaxSearchLengthForBoundariesTest, test_calc_max_search_length_for_boundaries) +{ + const auto p = GetParam(); + const auto trajectory = create_trajectory(p.trajectory_points); + + const auto max_search_length = + autoware::lane_departure_checker::utils::calcMaxSearchLengthForBoundaries( + trajectory, vehicle_info); + + EXPECT_DOUBLE_EQ(max_search_length, p.expected_max_search_length); +} + +INSTANTIATE_TEST_SUITE_P( + LaneDepartureCheckerTest, CalcMaxSearchLengthForBoundariesTest, + ::testing::Values( + CalcMaxSearchLengthForBoundariesParam{ + "EmptyTrajectory", + {}, + std::hypot(front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m)}, + CalcMaxSearchLengthForBoundariesParam{ + "SinglePointTrajectory", + {{0.0, 0.0}}, + std::hypot(front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m)}, + CalcMaxSearchLengthForBoundariesParam{ + "MultiPointTrajectory", + {{0.0, 0.0}, {1.0, 0.0}}, + 1.0 + std::hypot(front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m)}), + ::testing::PrintToStringParamName()); diff --git a/control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp b/control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp new file mode 100644 index 0000000000000..ccb85d1a0fec1 --- /dev/null +++ b/control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp @@ -0,0 +1,101 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/lane_departure_checker/utils.hpp" + +#include + +#include + +#include +#include + +using autoware::universe_utils::PoseDeviation; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +namespace +{ +Trajectory create_trajectory(const std::vector & points) +{ + Trajectory trajectory; + for (const auto & point : points) { + TrajectoryPoint p; + p.pose.position.x = point.x(); + p.pose.position.y = point.y(); + trajectory.points.push_back(p); + } + return trajectory; +} + +geometry_msgs::msg::Pose create_pose(const Eigen::Vector3d & x_y_yaw) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x_y_yaw[0]; + pose.position.y = x_y_yaw[1]; + pose.orientation.z = std::sin(x_y_yaw[2] / 2); + pose.orientation.w = std::cos(x_y_yaw[2] / 2); + return pose; +} + +constexpr double ego_nearest_dist_threshold = 3.0; +constexpr double ego_nearest_yaw_threshold = 1.046; +} // namespace + +struct CalcTrajectoryDeviationTestParam +{ + std::string description; + std::vector trajectory_points; + Eigen::Vector3d x_y_yaw; + bool exception_expected; + PoseDeviation expected_deviation; +}; + +std::ostream & operator<<(std::ostream & os, const CalcTrajectoryDeviationTestParam & p) +{ + return os << p.description; +} + +class CalcTrajectoryDeviationTest +: public ::testing::TestWithParam +{ +}; + +TEST_P(CalcTrajectoryDeviationTest, test_calc_trajectory_deviation) +{ + const auto p = GetParam(); + const auto trajectory = create_trajectory(p.trajectory_points); + const auto pose = create_pose(p.x_y_yaw); + if (p.exception_expected) { + EXPECT_ANY_THROW({ + autoware::lane_departure_checker::utils::calcTrajectoryDeviation( + trajectory, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + }); + } else { + const auto deviation = autoware::lane_departure_checker::utils::calcTrajectoryDeviation( + trajectory, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + + EXPECT_DOUBLE_EQ(deviation.lateral, p.expected_deviation.lateral); + EXPECT_DOUBLE_EQ(deviation.longitudinal, p.expected_deviation.longitudinal); + EXPECT_DOUBLE_EQ(deviation.yaw, p.expected_deviation.yaw); + } +} + +INSTANTIATE_TEST_SUITE_P( + LaneDepartureCheckerTest, CalcTrajectoryDeviationTest, + ::testing::Values( + CalcTrajectoryDeviationTestParam{"EmptyTrajectory", {}, {}, true, {}}, + CalcTrajectoryDeviationTestParam{ + "SinglePointTrajectory", {{0.0, 0.0}}, {0.0, 0.0, 0.0}, false, {0.0, 0.0, 0.0}}), + ::testing::PrintToStringParamName()); diff --git a/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp index 6189d8803a3ea..e17e2bc697272 100644 --- a/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp +++ b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp @@ -18,6 +18,9 @@ #include +#include +#include + using autoware::universe_utils::LinearRing2d; using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::PoseWithCovariance; diff --git a/control/autoware_lane_departure_checker/test/test_create_vehicle_passing_areas.cpp b/control/autoware_lane_departure_checker/test/test_create_vehicle_passing_areas.cpp new file mode 100644 index 0000000000000..40a4db4024094 --- /dev/null +++ b/control/autoware_lane_departure_checker/test/test_create_vehicle_passing_areas.cpp @@ -0,0 +1,166 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/lane_departure_checker/utils.hpp" + +#include +#include + +#include +#include +#include +#include + +#include + +#include + +using autoware::lane_departure_checker::utils::createVehiclePassingAreas; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::Point2d; + +class CreateVehiclePassingAreasTest : public ::testing::Test +{ +protected: + LinearRing2d createSquare(double x, double y, double size) + { + LinearRing2d square; + square.reserve(5); + square.push_back(Point2d{x, y}); // bottom-left + square.push_back(Point2d{x, y + size}); // top-left + square.push_back(Point2d{x + size, y + size}); // top-right + square.push_back(Point2d{x + size, y}); // bottom-right + square.push_back(Point2d{x, y}); // close the square + boost::geometry::correct(square); + return square; + } + + bool isPointInsideHull(const Point2d & point, const LinearRing2d & hull) + { + return boost::geometry::within(point, hull) || boost::geometry::covered_by(point, hull); + } + + /** + * @brief Validates that all points in the given footprints are contained within the hull + * + * For each footprint in the input vector, checks whether all its points (except the last one + * which is identical to the first point in a LinearRing2d) are contained within the given hull. + * This validation ensures the correctness of the convex hull generation for vehicle passing + * areas. + * + * @param footprints Vector of LinearRing2d representing vehicle footprints to validate + * Must not be empty and must not contain empty LinearRing2d + * @param hull LinearRing2d representing the convex hull that should contain all footprints + * @note The last point of each footprint is not checked as LinearRing2d is a closed ring + * where the last point is identical to the first point + * @pre footprints must not be empty + * @pre each LinearRing2d in footprints must not be empty, as it represents a closed ring + */ + void validateFootprintsInHull( + const std::vector & footprints, const LinearRing2d & hull) + { + for (const auto & footprint : footprints) { + // An empty footprint would be invalid and should fail the test + EXPECT_FALSE(footprint.empty()) << "Footprint must not be empty"; + if (footprint.empty()) { + continue; + } + + // Iterate up to size()-1 since LinearRing2d is a closed ring where + // the last point is the same as the first point, so we don't need + // to check it again + for (size_t i = 0; i < footprint.size() - 1; ++i) { + const auto & point = footprint[i]; + EXPECT_TRUE(isPointInsideHull(point, hull)) + << "Point (" << point.x() << ", " << point.y() << ") is not inside the hull"; + } + } + } + + void SetUp() override + { + /* + Square placement: + Y-axis + ^ + | + 1 +---+ +---+ +---+ + | |s1 | |s2 | |s3 | + 0 +---+ +---+ +---+ + | + +---+---+---+---+---+---+--> X-axis + 0 1 1 2 3 4 + + s1: square1_ from (0,0) to (1,1) + s2: square2_ from (1,0) to (2,1) - adjacent to s1 + s3: square3_ from (3,0) to (4,1) - 1 unit apart from s2 + */ + square1_ = createSquare(0.0, 0.0, 1.0); // 1x1 square at origin + square2_ = createSquare(1.0, 0.0, 1.0); // square adjacent to square1_ + square3_ = createSquare(3.0, 0.0, 1.0); // square one unit apart from square2_ + } + + LinearRing2d square1_; // Reference square (0,0) + LinearRing2d square2_; // Adjacent square (1,0) + LinearRing2d square3_; // Distant square (3,0) +}; + +TEST_F(CreateVehiclePassingAreasTest, ReturnsEmptyAreaForEmptyInput) +{ + const std::vector empty_footprints; + const auto areas = createVehiclePassingAreas(empty_footprints); + EXPECT_TRUE(areas.empty()); +} + +TEST_F(CreateVehiclePassingAreasTest, ReturnsSameAreaForSingleFootprint) +{ + const std::vector single_footprint = {square1_}; + const auto areas = createVehiclePassingAreas(single_footprint); + + ASSERT_EQ(areas.size(), 1); + + auto result = areas.front(); + boost::geometry::correct(result); + EXPECT_EQ(result, square1_); +} + +TEST_F(CreateVehiclePassingAreasTest, CreatesValidHullForAdjacentFootprints) +{ + const std::vector footprints = {square1_, square2_}; + auto areas = createVehiclePassingAreas(footprints); + + ASSERT_EQ(areas.size(), 1); + auto & hull = areas.front(); + boost::geometry::correct(hull); + + // Basic validation of the convex hull + EXPECT_GE(hull.size(), 5); // At least a quadrilateral plus closing point + + // Verify all points from original footprints are inside the hull + validateFootprintsInHull(footprints, hull); +} + +TEST_F(CreateVehiclePassingAreasTest, HandlesNonAdjacentFootprints) +{ + const std::vector footprints = { + square1_, square3_}; // Using square3_ which is not adjacent to square1_ + auto areas = createVehiclePassingAreas(footprints); + + ASSERT_EQ(areas.size(), 1); + auto & hull = areas.front(); + boost::geometry::correct(hull); + + // Verify all points are inside the hull even for non-adjacent squares + validateFootprintsInHull(footprints, hull); +} diff --git a/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp b/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp index 67fe69323584e..214a165660225 100644 --- a/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp +++ b/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp @@ -18,6 +18,9 @@ #include +#include +#include + using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index eedac39f37bd4..81c8b71683092 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -227,8 +227,6 @@ class MPC bool m_is_forward_shift = true; // Flag indicating if the shift is in the forward direction. - double m_min_prediction_length = 5.0; // Minimum prediction distance. - rclcpp::Publisher::SharedPtr m_debug_frenet_predicted_trajectory_pub; rclcpp::Publisher::SharedPtr m_debug_resampled_reference_trajectory_pub; /** @@ -405,7 +403,7 @@ class MPC template inline bool fail_warn_throttle(Args &&... args) const { - RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, args...); + RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, "%s", args...); return false; } @@ -413,7 +411,7 @@ class MPC template inline void warn_throttle(Args &&... args) const { - RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, args...); + RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, "%s", args...); } public: diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp index fe68d150b5b9a..d7442f64b028a 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -292,13 +292,13 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase template inline void info_throttle(Args &&... args) { - RCLCPP_INFO_THROTTLE(logger_, *clock_, 5000, args...); + RCLCPP_INFO_THROTTLE(logger_, *clock_, 5000, "%s", args...); } template inline void warn_throttle(Args &&... args) { - RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, args...); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "%s", args...); } }; } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp index 4a056c351208b..379ec0b614712 100644 --- a/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp @@ -14,6 +14,7 @@ #include "autoware/mpc_lateral_controller/lowpass_filter.hpp" +#include #include namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 3c3496080b597..4c3c8bec9b12e 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -23,7 +23,11 @@ #include #include +#include #include +#include +#include +#include namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp index 842689527c847..0156d7af138bc 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp @@ -14,6 +14,8 @@ #include "autoware/mpc_lateral_controller/mpc_trajectory.hpp" +#include + namespace autoware::motion::control::mpc_lateral_controller { void MPCTrajectory::push_back( diff --git a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 820ee848c17fe..e8bc981a8ade8 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -21,8 +21,10 @@ #include "autoware/universe_utils/math/normalization.hpp" #include +#include #include #include +#include #include namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp index 97b26162812ef..789d5602b2414 100644 --- a/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp +++ b/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp @@ -29,6 +29,7 @@ #include #include +#include namespace { diff --git a/control/autoware_operation_mode_transition_manager/src/data.cpp b/control/autoware_operation_mode_transition_manager/src/data.cpp index 1fac496f3c71c..48200d75fe2cb 100644 --- a/control/autoware_operation_mode_transition_manager/src/data.cpp +++ b/control/autoware_operation_mode_transition_manager/src/data.cpp @@ -14,6 +14,8 @@ #include "data.hpp" +#include + namespace autoware::operation_mode_transition_manager { diff --git a/control/autoware_operation_mode_transition_manager/src/node.cpp b/control/autoware_operation_mode_transition_manager/src/node.cpp index d0c7f303fca04..25c048b0540fa 100644 --- a/control/autoware_operation_mode_transition_manager/src/node.cpp +++ b/control/autoware_operation_mode_transition_manager/src/node.cpp @@ -16,6 +16,8 @@ #include +#include + namespace autoware::operation_mode_transition_manager { diff --git a/control/autoware_operation_mode_transition_manager/src/state.cpp b/control/autoware_operation_mode_transition_manager/src/state.cpp index 31cd32e6311a9..5969e6b9fb03d 100644 --- a/control/autoware_operation_mode_transition_manager/src/state.cpp +++ b/control/autoware_operation_mode_transition_manager/src/state.cpp @@ -22,6 +22,8 @@ #include #include +#include +#include namespace autoware::operation_mode_transition_manager { diff --git a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 134de5d1ac8bd..fe500d2551623 100644 --- a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -19,6 +19,8 @@ #include // NOLINT +#include + #ifdef ROS_DISTRO_GALACTIC #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #else diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp index 085addbe17fc9..f3a7bc3c1333d 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp @@ -39,6 +39,7 @@ #include #include #include +#include namespace { diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp index ed9579f296fea..55a3680dfaaf0 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp @@ -156,9 +156,9 @@ int32_t PurePursuit::findNextPointIdx(int32_t search_start_idx) } // look for the next waypoint. - for (int32_t i = search_start_idx; i < (int32_t)curr_wps_ptr_->size(); i++) { + for (int32_t i = search_start_idx; i < static_cast(curr_wps_ptr_->size()); i++) { // if search waypoint is the last - if (i == ((int32_t)curr_wps_ptr_->size() - 1)) { + if (i == (static_cast(curr_wps_ptr_->size()) - 1)) { return i; } diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp index 9b68594f167e8..6688ed6a9f27d 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp @@ -15,6 +15,8 @@ #include "autoware/pure_pursuit/util/interpolate.hpp" #include +#include +#include #include namespace autoware::pure_pursuit diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/src/proxima_calc.cpp b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/src/proxima_calc.cpp index 8dbee65804237..daa054af31627 100644 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/src/proxima_calc.cpp +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/src/proxima_calc.cpp @@ -19,7 +19,9 @@ #include #include +#include #include +#include namespace py = pybind11; Eigen::VectorXd tanh(const Eigen::VectorXd & v) diff --git a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml index e458edaf6c471..0de08076b8c06 100644 --- a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml +++ b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml @@ -258,7 +258,7 @@ - + @@ -268,7 +268,7 @@ - + @@ -286,7 +286,7 @@ - + diff --git a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp index 40e4366d9aa17..0b1dac644a8ab 100644 --- a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -30,6 +30,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include #include #include diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index a18571144792a..c540f48dfc186 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -16,6 +16,7 @@ #include #include +#include namespace autoware::vehicle_cmd_gate { diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 9e5db63f9311d..c44e96631998f 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -137,7 +137,6 @@ class VehicleCmdGate : public rclcpp::Node bool is_engaged_; bool is_system_emergency_ = false; bool is_external_emergency_stop_ = false; - bool is_filtered_marker_published_ = false; double current_steer_ = 0; GateMode current_gate_mode_; MrmState current_mrm_state_; diff --git a/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 66dde51780ffa..9b403e2f8ed74 100644 --- a/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -17,9 +17,11 @@ #include +#include #include #include #include +#include #include #include diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index 3895b3cc13465..e87efbe179eee 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -44,34 +44,34 @@ Error acceleration calculations are made based on the velocity calculations abov #### control_performance_analysis::msg::DrivingMonitorStamped -| Name | Type | Description | -| ---------------------------- | ----- | -------------------------------------------------------- | -| `longitudinal_acceleration` | float | [m / s^2] | -| `longitudinal_jerk` | float | [m / s^3] | -| `lateral_acceleration` | float | [m / s^2] | -| `lateral_jerk` | float | [m / s^3] | -| `desired_steering_angle` | float | [rad] | -| `controller_processing_time` | float | Timestamp between last two control command messages [ms] | +| Name | Type | Description | +| ---------------------------- | ----- | --------------------------------------------------------------------- | +| `longitudinal_acceleration` | float | $[ \mathrm{m/s^2} ]$ | +| `longitudinal_jerk` | float | $[ \mathrm{m/s^3} ]$ | +| `lateral_acceleration` | float | $[ \mathrm{m/s^2} ]$ | +| `lateral_jerk` | float | $[ \mathrm{m/s^3} ]$ | +| `desired_steering_angle` | float | $[ \mathrm{rad} ]$ | +| `controller_processing_time` | float | Timestamp between last two control command messages $[ \mathrm{ms} ]$ | #### control_performance_analysis::msg::ErrorStamped -| Name | Type | Description | -| ------------------------------------------ | ----- | ----------------------------------------------------------------------------------------------------------------- | -| `lateral_error` | float | [m] | -| `lateral_error_velocity` | float | [m / s] | -| `lateral_error_acceleration` | float | [m / s^2] | -| `longitudinal_error` | float | [m] | -| `longitudinal_error_velocity` | float | [m / s] | -| `longitudinal_error_acceleration` | float | [m / s^2] | -| `heading_error` | float | [rad] | -| `heading_error_velocity` | float | [rad / s] | -| `control_effort_energy` | float | [u * R * u^T] | -| `error_energy` | float | lateral_error^2 + heading_error^2 | -| `value_approximation` | float | V = xPx' ; Value function from DARE Lyap matrix P | -| `curvature_estimate` | float | [1 / m] | -| `curvature_estimate_pp` | float | [1 / m] | -| `vehicle_velocity_error` | float | [m / s] | -| `tracking_curvature_discontinuity_ability` | float | Measures the ability to tracking the curvature changes [`abs(delta(curvature)) / (1 + abs(delta(lateral_error))`] | +| Name | Type | Description | +| ------------------------------------------ | ----- | ----------------------------------------------------------------------------------------------------------------------------------------- | +| `lateral_error` | float | $[ \mathrm{m} ]$ | +| `lateral_error_velocity` | float | $[ \mathrm{m/s} ]$ | +| `lateral_error_acceleration` | float | $[ \mathrm{m/s^2} ]$ | +| `longitudinal_error` | float | $[ \mathrm{m} ]$ | +| `longitudinal_error_velocity` | float | $[ \mathrm{m/s} ]$ | +| `longitudinal_error_acceleration` | float | $[ \mathrm{m/s^2} ]$ | +| `heading_error` | float | $[ \mathrm{rad} ]$ | +| `heading_error_velocity` | float | $[ \mathrm{rad/s} ]$ | +| `control_effort_energy` | float | $[ \mathbf{u}^\top \mathbf{R} \mathbf{u} ]$ simplified to $[ R \cdot u^2 ]$ | +| `error_energy` | float | $e_{\text{lat}}^2 + e_\theta^2$ (squared lateral error + squared heading error) | +| `value_approximation` | float | $V = \mathbf{x}^\top \mathbf{P} \mathbf{x}$; Value function from DARE Lyapunov matrix $\mathbf{P}$ | +| `curvature_estimate` | float | $[ \mathrm{1/m} ]$ | +| `curvature_estimate_pp` | float | $[ \mathrm{1/m} ]$ | +| `vehicle_velocity_error` | float | $[ \mathrm{m/s} ]$ | +| `tracking_curvature_discontinuity_ability` | float | Measures the ability to track curvature changes $\frac{\lvert \Delta(\text{curvature}) \rvert}{1 + \lvert \Delta(e_{\text{lat}}) \rvert}$ | ## Parameters diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 4f149a3846595..203357d3f0474 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -42,8 +42,8 @@ tf2_eigen tf2_ros tier4_debug_msgs + autoware_global_parameter_loader builtin_interfaces - global_parameter_loader rosidl_default_runtime ament_lint_auto autoware_lint_common 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 769593ac34d09..08f736dd0b630 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp index 7771f59d15454..2841888b61bcc 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include 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 4b6d0755db25f..12e38fd9968ab 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 @@ -18,6 +18,8 @@ #include #include +#include + namespace utils { diff --git a/docs/assets/js/mathjax.js b/docs/assets/js/mathjax.js index 117b04607f279..adb184a80399e 100644 --- a/docs/assets/js/mathjax.js +++ b/docs/assets/js/mathjax.js @@ -1,3 +1,7 @@ +// This file is automatically synced from: +// https://github.com/autowarefoundation/sync-file-templates +// To make changes, update the source repository and follow the guidelines in its README. + window.MathJax = { tex: { inlineMath: [["\\(", "\\)"]], diff --git a/evaluator/autoware_control_evaluator/README.md b/evaluator/autoware_control_evaluator/README.md index 59c09015bd7b5..b7da418764959 100644 --- a/evaluator/autoware_control_evaluator/README.md +++ b/evaluator/autoware_control_evaluator/README.md @@ -4,14 +4,14 @@ This package provides nodes that generate metrics to evaluate the quality of control. -It publishes diagnostic information about control modules' outputs as well as the ego vehicle's current kinematics and position. +It publishes metric information about control modules' outputs as well as the ego vehicle's current kinematics and position. ## Evaluated metrics -The control evaluator uses the metrics defined in `include/autoware/control_evaluator/metrics/deviation_metrics.hpp`to calculate deviations in yaw and lateral distance from the ego's set-point. The control_evaluator can also be customized to offer metrics/evaluation about other control modules. Currently, the control_evaluator offers a simple diagnostic output based on the autonomous_emergency_braking node's output, but this functionality can be extended to evaluate other control modules' performance. +The control evaluator uses the metrics defined in `include/autoware/control_evaluator/metrics/metric.hpp`to calculate deviations in yaw and lateral distance from the ego's set-point. The control_evaluator can also be customized to offer metrics/evaluation about other control modules. Currently, the control_evaluator offers a simple metric output based on the autonomous_emergency_braking node's output, but this functionality can be extended to evaluate other control modules' performance. ## Kinematics output -The control evaluator module also constantly publishes information regarding the ego vehicle's kinematics and position. It publishes the current ego lane id with the longitudinal `s` and lateral `t` arc coordinates. It also publishes the current ego speed, acceleration and jerk in its diagnostic messages. +The control evaluator module also constantly publishes information regarding the ego vehicle's kinematics and position. It publishes the current ego lane id with the longitudinal `s` and lateral `t` arc coordinates. It also publishes the current ego speed, acceleration and jerk in its metric messages. This information can be used by other nodes to establish automated evaluation using rosbags: by crosschecking the ego position and kinematics with the evaluated control module's output, it is possible to judge if the evaluated control modules reacted in a satisfactory way at certain interesting points of the rosbag reproduction. diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 6e4a04964a72c..c510b2ea46779 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -16,6 +16,8 @@ #define AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/control_evaluator/metrics/metric.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include #include @@ -32,10 +34,11 @@ #include #include #include +#include namespace control_diagnostics { - +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -53,6 +56,9 @@ class ControlEvaluatorNode : public rclcpp::Node { public: explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options); + ~ControlEvaluatorNode() override; + + void AddMetricMsg(const Metric & metric, const double & metric_value); void AddLateralDeviationMetricMsg(const Trajectory & traj, const Point & ego_point); void AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose); void AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose); @@ -85,9 +91,18 @@ class ControlEvaluatorNode : public rclcpp::Node // update Route Handler void getRouteData(); - // Calculator - // Metrics - std::deque stamps_; + // Parameters + bool output_metrics_; + + // Metric + const std::vector metrics_ = { + // collect all metrics + Metric::lateral_deviation, Metric::yaw_deviation, Metric::goal_longitudinal_deviation, + Metric::goal_lateral_deviation, Metric::goal_yaw_deviation, + }; + + std::array, static_cast(Metric::SIZE)> + metric_accumulators_; // 3(min, max, mean) * metric_size MetricArrayMsg metrics_msg_; autoware::route_handler::RouteHandler route_handler_; diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp new file mode 100644 index 0000000000000..be2f3135d7f7e --- /dev/null +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp @@ -0,0 +1,81 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__CONTROL_EVALUATOR__METRICS__METRIC_HPP_ +#define AUTOWARE__CONTROL_EVALUATOR__METRICS__METRIC_HPP_ + +#include +#include +#include +#include + +namespace control_diagnostics +{ +/** + * @brief Enumeration of trajectory metrics + */ +enum class Metric { + lateral_deviation, + yaw_deviation, + goal_longitudinal_deviation, + goal_lateral_deviation, + goal_yaw_deviation, + SIZE, +}; + +static const std::unordered_map str_to_metric = { + {"lateral_deviation", Metric::lateral_deviation}, + {"yaw_deviation", Metric::yaw_deviation}, + {"goal_longitudinal_deviation", Metric::goal_longitudinal_deviation}, + {"goal_lateral_deviation", Metric::goal_lateral_deviation}, + {"goal_yaw_deviation", Metric::goal_yaw_deviation}, +}; + +static const std::unordered_map metric_to_str = { + {Metric::lateral_deviation, "lateral_deviation"}, + {Metric::yaw_deviation, "yaw_deviation"}, + {Metric::goal_longitudinal_deviation, "goal_longitudinal_deviation"}, + {Metric::goal_lateral_deviation, "goal_lateral_deviation"}, + {Metric::goal_yaw_deviation, "goal_yaw_deviation"}, +}; + +// Metrics descriptions +static const std::unordered_map metric_descriptions = { + {Metric::lateral_deviation, "Lateral deviation from the reference trajectory[m]"}, + {Metric::yaw_deviation, "Yaw deviation from the reference trajectory[rad]"}, + {Metric::goal_longitudinal_deviation, "Longitudinal deviation from the goal point[m]"}, + {Metric::goal_lateral_deviation, "Lateral deviation from the goal point[m]"}, + {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"}}; + +namespace details +{ +static struct CheckCorrectMaps +{ + CheckCorrectMaps() + { + if ( + str_to_metric.size() != static_cast(Metric::SIZE) || + metric_to_str.size() != static_cast(Metric::SIZE) || + metric_descriptions.size() != static_cast(Metric::SIZE)) { + std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: "; + std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " " + << metric_descriptions.size() << std::endl; + } + } +} check; + +} // namespace details +} // namespace control_diagnostics + +#endif // AUTOWARE__CONTROL_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index e9fee7ffadbf2..4cf795afb5a7d 100644 --- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -1,20 +1,20 @@ - + - - + + + - - + - - + + diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 8ab2c73cb619a..1c724fc7bd0ec 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -22,6 +22,7 @@ autoware_test_utils autoware_universe_utils nav_msgs + nlohmann-json-dev pluginlib rclcpp rclcpp_components diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index c24dfee571852..9c8579469f878 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -16,7 +16,11 @@ #include #include +#include +#include +#include +#include #include #include #include @@ -33,12 +37,63 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti this->create_publisher("~/debug/processing_time_ms", 1); metrics_pub_ = create_publisher("~/metrics", 1); + // Parameters + output_metrics_ = declare_parameter("output_metrics"); + // Timer callback to publish evaluator diagnostics using namespace std::literals::chrono_literals; timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ControlEvaluatorNode::onTimer, this)); } +ControlEvaluatorNode::~ControlEvaluatorNode() +{ + if (!output_metrics_) { + return; + } + + // generate json data + using json = nlohmann::json; + json j; + for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; + j[base_name + "min"] = metric_accumulators_[static_cast(metric)].min(); + j[base_name + "max"] = metric_accumulators_[static_cast(metric)].max(); + j[base_name + "mean"] = metric_accumulators_[static_cast(metric)].mean(); + j[base_name + "count"] = metric_accumulators_[static_cast(metric)].count(); + j[base_name + "description"] = metric_descriptions.at(metric); + } + + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } + } + + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_control_evaluator-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } +} + void ControlEvaluatorNode::getRouteData() { // route @@ -62,6 +117,18 @@ void ControlEvaluatorNode::getRouteData() } } +void ControlEvaluatorNode::AddMetricMsg(const Metric & metric, const double & metric_value) +{ + MetricMsg metric_msg; + metric_msg.name = metric_to_str.at(metric); + metric_msg.value = std::to_string(metric_value); + metrics_msg_.metric_array.push_back(metric_msg); + + if (output_metrics_) { + metric_accumulators_[static_cast(metric)].add(metric_value); + } +} + void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose) { const auto current_lanelets = [&]() { @@ -97,7 +164,6 @@ void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose) metric_msg.value = std::to_string(arc_coordinates.distance); metrics_msg_.metric_array.push_back(metric_msg); } - return; } void ControlEvaluatorNode::AddKinematicStateMetricMsg( @@ -141,59 +207,44 @@ void ControlEvaluatorNode::AddKinematicStateMetricMsg( void ControlEvaluatorNode::AddLateralDeviationMetricMsg( const Trajectory & traj, const Point & ego_point) { - const double lateral_deviation = metrics::calcLateralDeviation(traj, ego_point); + const Metric metric = Metric::lateral_deviation; + const double metric_value = metrics::calcLateralDeviation(traj, ego_point); - MetricMsg metric_msg; - metric_msg.name = "lateral_deviation"; - metric_msg.value = std::to_string(lateral_deviation); - metrics_msg_.metric_array.push_back(metric_msg); - return; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose) { - const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose); + const Metric metric = Metric::yaw_deviation; + const double metric_value = metrics::calcYawDeviation(traj, ego_pose); - MetricMsg metric_msg; - metric_msg.name = "yaw_deviation"; - metric_msg.value = std::to_string(yaw_deviation); - metrics_msg_.metric_array.push_back(metric_msg); - return; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose) { - const double longitudinal_deviation = + const Metric metric = Metric::goal_longitudinal_deviation; + const double metric_value = metrics::calcLongitudinalDeviation(route_handler_.getGoalPose(), ego_pose.position); - MetricMsg metric_msg; - metric_msg.name = "goal_longitudinal_deviation"; - metric_msg.value = std::to_string(longitudinal_deviation); - metrics_msg_.metric_array.push_back(metric_msg); - return; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg(const Pose & ego_pose) { - const double lateral_deviation = + const Metric metric = Metric::goal_lateral_deviation; + const double metric_value = metrics::calcLateralDeviation(route_handler_.getGoalPose(), ego_pose.position); - MetricMsg metric_msg; - metric_msg.name = "goal_lateral_deviation"; - metric_msg.value = std::to_string(lateral_deviation); - metrics_msg_.metric_array.push_back(metric_msg); - return; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg(const Pose & ego_pose) { - const double yaw_deviation = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose); + const Metric metric = Metric::goal_yaw_deviation; + const double metric_value = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose); - MetricMsg metric_msg; - metric_msg.name = "goal_yaw_deviation"; - metric_msg.value = std::to_string(yaw_deviation); - metrics_msg_.metric_array.push_back(metric_msg); - return; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::onTimer() diff --git a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp index 0a2f777e49b61..9098ce5667424 100644 --- a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp @@ -28,6 +28,7 @@ #include +#include #include #include #include @@ -52,6 +53,7 @@ class EvalTest : public ::testing::Test rclcpp::NodeOptions options; const auto share_dir = ament_index_cpp::get_package_share_directory("autoware_control_evaluator"); + options.arguments({"--ros-args", "-p", "output_metrics:=false"}); dummy_node = std::make_shared("control_evaluator_test_node"); eval_node = std::make_shared(options); diff --git a/evaluator/autoware_planning_evaluator/README.md b/evaluator/autoware_planning_evaluator/README.md index d09949bd69b0e..4fcdf4d7e55fd 100644 --- a/evaluator/autoware_planning_evaluator/README.md +++ b/evaluator/autoware_planning_evaluator/README.md @@ -20,7 +20,7 @@ which is also responsible for calculating metrics. ### Stat -Each metric is calculated using a `Stat` instance which contains +Each metric is calculated using a `autoware::universe_utils::Accumulator` instance which contains the minimum, maximum, and mean values calculated for the metric as well as the number of values measured. @@ -35,7 +35,7 @@ The `MetricsCalculator` is responsible for calculating metric statistics through calls to function: ```C++ -Stat MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const; +Accumulator MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const; ``` Adding a new metric `M` requires the following steps: @@ -62,8 +62,8 @@ Each metric is published on a topic named after the metric name. | ----------- | ------------------------------------- | ----------------------------------------------------------------- | | `~/metrics` | `tier4_metric_msgs::msg::MetricArray` | MetricArray with many metrics of `tier4_metric_msgs::msg::Metric` | -When shut down, the evaluation node writes the values of the metrics measured during its lifetime -to a file as specified by the `output_file` parameter. +If `output_metrics = true`, the evaluation node writes the statics of the metrics measured during its lifetime +to `/autoware_metrics/-.json` when shut down. ## Parameters diff --git a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml index 2e92a174ca79f..14c1bcc6beea4 100644 --- a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml +++ b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml @@ -1,6 +1,5 @@ /**: ros__parameters: - output_file: "" # if empty, metrics are not written to file ego_frame: base_link # reference frame of ego selected_metrics: diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp index 8d7cfbd30d604..59866c96ad702 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -24,6 +24,7 @@ namespace planning_diagnostics { namespace metrics { +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; @@ -35,7 +36,7 @@ using geometry_msgs::msg::Pose; * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj); +Accumulator calcLateralDeviation(const Trajectory & ref, const Trajectory & traj); /** * @brief calculate yaw deviation of the given trajectory from the reference trajectory @@ -43,7 +44,7 @@ Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & tra * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj); +Accumulator calcYawDeviation(const Trajectory & ref, const Trajectory & traj); /** * @brief calculate velocity deviation of the given trajectory from the reference trajectory @@ -51,7 +52,7 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj); * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj); +Accumulator calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj); /** * @brief calculate longitudinal deviation of the given ego pose from the modified goal pose @@ -59,7 +60,7 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr * @param [in] target_point target point * @return calculated statistics */ -Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point); +Accumulator calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point); /** * @brief calculate lateral deviation of the given ego pose from the modified goal pose @@ -67,7 +68,7 @@ Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & tar * @param [in] target_point target point * @return calculated statistics */ -Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point); +Accumulator calcLateralDeviation(const Pose & base_pose, const Point & target_point); /** * @brief calculate yaw deviation of the given ego pose from the modified goal pose @@ -75,7 +76,7 @@ Stat calcLateralDeviation(const Pose & base_pose, const Point & target_p * @param [in] target_pose target pose * @return calculated statistics */ -Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose); +Accumulator calcYawDeviation(const Pose & base_pose, const Pose & target_pose); } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp index 2ade316ba7ad5..0006e49c3338a 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp @@ -15,8 +15,6 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ -#include "autoware/planning_evaluator/stat.hpp" - #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp index c4d468b4dacd5..55e46bf710450 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -24,6 +24,7 @@ namespace planning_diagnostics { namespace metrics { +using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; @@ -33,7 +34,8 @@ using autoware_planning_msgs::msg::Trajectory; * @param [in] traj trajectory * @return calculated statistics */ -Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj); +Accumulator calcDistanceToObstacle( + const PredictedObjects & obstacles, const Trajectory & traj); /** * @brief calculate the time to collision of the trajectory with the given obstacles @@ -42,7 +44,7 @@ Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Tr * @param [in] traj trajectory * @return calculated statistics */ -Stat calcTimeToCollision( +Accumulator calcTimeToCollision( const PredictedObjects & obstacles, const Trajectory & traj, const double distance_threshold); } // namespace metrics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp index f1e93380d34d6..38f388feeadda 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -23,6 +23,7 @@ namespace planning_diagnostics { namespace metrics { +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; /** @@ -31,7 +32,7 @@ using autoware_planning_msgs::msg::Trajectory; * @param [in] traj2 second trajectory * @return calculated statistics */ -Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2); +Accumulator calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2); /** * @brief calculate the lateral distance between two trajectories @@ -39,7 +40,7 @@ Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & tr * @param [in] traj2 second trajectory * @return calculated statistics */ -Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2); +Accumulator calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2); } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp index 14a6a5c451619..c530a8d6d8b05 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -24,6 +24,7 @@ namespace planning_diagnostics { namespace metrics { +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -33,56 +34,57 @@ using autoware_planning_msgs::msg::TrajectoryPoint; * @param [in] min_dist_threshold minimum distance between successive points * @return calculated statistics */ -Stat calcTrajectoryRelativeAngle(const Trajectory & traj, const double min_dist_threshold); +Accumulator calcTrajectoryRelativeAngle( + const Trajectory & traj, const double min_dist_threshold); /** * @brief calculate metric for the distance between trajectory points * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryInterval(const Trajectory & traj); +Accumulator calcTrajectoryInterval(const Trajectory & traj); /** * @brief calculate curvature metric * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryCurvature(const Trajectory & traj); +Accumulator calcTrajectoryCurvature(const Trajectory & traj); /** * @brief calculate length of the trajectory [m] * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryLength(const Trajectory & traj); +Accumulator calcTrajectoryLength(const Trajectory & traj); /** * @brief calculate duration of the trajectory [s] * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryDuration(const Trajectory & traj); +Accumulator calcTrajectoryDuration(const Trajectory & traj); /** * @brief calculate velocity metrics for the trajectory * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryVelocity(const Trajectory & traj); +Accumulator calcTrajectoryVelocity(const Trajectory & traj); /** * @brief calculate acceleration metrics for the trajectory * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryAcceleration(const Trajectory & traj); +Accumulator calcTrajectoryAcceleration(const Trajectory & traj); /** * @brief calculate jerk metrics for the trajectory * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryJerk(const Trajectory & traj); +Accumulator calcTrajectoryJerk(const Trajectory & traj); } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp index f500a435edba1..97d23cad10af2 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ #include "autoware/planning_evaluator/metrics/metric.hpp" #include "autoware/planning_evaluator/parameters.hpp" -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" @@ -28,6 +28,7 @@ namespace planning_diagnostics { +using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using autoware_planning_msgs::msg::Trajectory; @@ -47,8 +48,8 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return string describing the requested metric */ - std::optional> calculate(const Metric metric, const Trajectory & traj) const; - std::optional> calculate( + std::optional> calculate(const Metric metric, const Trajectory & traj) const; + std::optional> calculate( const Metric metric, const Pose & base_pose, const Pose & target_pose) const; /** diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp index ed9c975436ba5..7eed6e5645abc 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ #include "autoware/planning_evaluator/metrics_calculator.hpp" -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -33,6 +33,7 @@ namespace planning_diagnostics { +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -60,7 +61,7 @@ class MotionEvaluatorNode : public rclcpp::Node std::unique_ptr tf_listener_ptr_; // Parameters - std::string output_file_str_; + bool output_metrics_; // Calculator MetricsCalculator metrics_calculator_; diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 065d168b02d38..9c268206846d9 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ #include "autoware/planning_evaluator/metrics_calculator.hpp" -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -43,6 +43,7 @@ #include namespace planning_diagnostics { +using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using autoware_planning_msgs::msg::Trajectory; @@ -60,7 +61,7 @@ class PlanningEvaluatorNode : public rclcpp::Node { public: explicit PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options); - ~PlanningEvaluatorNode(); + ~PlanningEvaluatorNode() override; /** * @brief callback on receiving an odometry @@ -96,17 +97,17 @@ class PlanningEvaluatorNode : public rclcpp::Node const Odometry::ConstSharedPtr ego_state_ptr); /** - * @brief publish the given metric statistic + * @brief add the given metric statistic */ - void AddMetricMsg(const Metric & metric, const Stat & metric_stat); + void AddMetricMsg(const Metric & metric, const Accumulator & metric_stat); /** - * @brief publish current ego lane info + * @brief add current ego lane info */ void AddLaneletMetricMsg(const Odometry::ConstSharedPtr ego_state_ptr); /** - * @brief publish current ego kinematic state + * @brief add current ego kinematic state */ void AddKinematicStateMetricMsg( const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr); @@ -154,15 +155,15 @@ class PlanningEvaluatorNode : public rclcpp::Node MetricArrayMsg metrics_msg_; // Parameters - std::string output_file_str_; + bool output_metrics_; std::string ego_frame_str_; // Calculator MetricsCalculator metrics_calculator_; // Metrics std::vector metrics_; - std::deque stamps_; - std::array>, static_cast(Metric::SIZE)> metric_stats_; + std::array, 3>, static_cast(Metric::SIZE)> + metric_accumulators_; // 3(min, max, mean) * metric_size rclcpp::TimerBase::SharedPtr timer_; std::optional prev_acc_stamped_{std::nullopt}; diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp deleted file mode 100644 index 5f63f29a96f26..0000000000000 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#ifndef AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_ -#define AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_ - -namespace planning_diagnostics -{ -/** - * @brief class to incrementally build statistics - * @typedef T type of the values (default to double) - */ -template -class Stat -{ -public: - /** - * @brief add a value - * @param value value to add - */ - void add(const T & value) - { - if (value < min_) { - min_ = value; - } - if (value > max_) { - max_ = value; - } - ++count_; - mean_ = mean_ + (value - mean_) / count_; - } - - /** - * @brief get the mean value - */ - long double mean() const { return mean_; } - - /** - * @brief get the minimum value - */ - T min() const { return min_; } - - /** - * @brief get the maximum value - */ - T max() const { return max_; } - - /** - * @brief get the number of values used to build this statistic - */ - unsigned int count() const { return count_; } - - template - friend std::ostream & operator<<(std::ostream & os, const Stat & stat); - -private: - T min_ = std::numeric_limits::max(); - T max_ = std::numeric_limits::min(); - long double mean_ = 0.0; - unsigned int count_ = 0; -}; - -/** - * @brief overload << operator for easy print to output stream - */ -template -std::ostream & operator<<(std::ostream & os, const Stat & stat) -{ - if (stat.count() == 0) { - os << "None None None"; - } else { - os << stat.min() << " " << stat.max() << " " << stat.mean(); - } - return os; -} - -} // namespace planning_diagnostics - -#endif // AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_ diff --git a/evaluator/autoware_planning_evaluator/launch/motion_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/motion_evaluator.launch.xml index 6558d0b568c94..042d2458ae1da 100644 --- a/evaluator/autoware_planning_evaluator/launch/motion_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/motion_evaluator.launch.xml @@ -1,7 +1,10 @@ + + + diff --git a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index 90c205dd55f1f..967668793a1e7 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -1,4 +1,6 @@ + + @@ -12,6 +14,7 @@ + diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 15d5d6da8504d..827784f71a823 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -22,6 +22,7 @@ eigen geometry_msgs nav_msgs + nlohmann-json-dev pluginlib rclcpp rclcpp_components diff --git a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json index e7efbe06a8c00..1ef3874a0dcbc 100644 --- a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json +++ b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json @@ -6,10 +6,10 @@ "autoware_planning_evaluator": { "type": "object", "properties": { - "output_file": { - "description": "output metrics file path", - "type": "string", - "default": "" + "output_metrics": { + "description": "If true, the evaluation node writes the metrics' statics to `/autoware_metrics/-.json` when the node shut down,", + "type": "boolean", + "default": "false" }, "ego_frame": { "description": "reference frame of ego", diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index 8ce8a009652d8..7e2217a49ef87 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -25,9 +25,9 @@ namespace metrics using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj) +Accumulator calcLateralDeviation(const Trajectory & ref, const Trajectory & traj) { - Stat stat; + Accumulator stat; if (ref.points.empty() || traj.points.empty()) { return stat; @@ -45,9 +45,9 @@ Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & tra return stat; } -Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) +Accumulator calcYawDeviation(const Trajectory & ref, const Trajectory & traj) { - Stat stat; + Accumulator stat; if (ref.points.empty() || traj.points.empty()) { return stat; @@ -64,9 +64,9 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) return stat; } -Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj) +Accumulator calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj) { - Stat stat; + Accumulator stat; if (ref.points.empty() || traj.points.empty()) { return stat; @@ -81,23 +81,23 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr return stat; } -Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) +Accumulator calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) { - Stat stat; + Accumulator stat; stat.add(std::abs(autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point))); return stat; } -Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point) +Accumulator calcLateralDeviation(const Pose & base_pose, const Point & target_point) { - Stat stat; + Accumulator stat; stat.add(std::abs(autoware::universe_utils::calcLateralDeviation(base_pose, target_point))); return stat; } -Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose) +Accumulator calcYawDeviation(const Pose & base_pose, const Pose & target_pose) { - Stat stat; + Accumulator stat; stat.add(std::abs(autoware::universe_utils::calcYawDeviation(base_pose, target_pose))); return stat; } diff --git a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp index 3cdaf3d7eaaae..0afc8d982a7e9 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -30,9 +30,10 @@ namespace metrics using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::TrajectoryPoint; -Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) +Accumulator calcDistanceToObstacle( + const PredictedObjects & obstacles, const Trajectory & traj) { - Stat stat; + Accumulator stat; for (const TrajectoryPoint & p : traj.points) { double min_dist = std::numeric_limits::max(); for (const auto & object : obstacles.objects) { @@ -45,10 +46,10 @@ Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Tr return stat; } -Stat calcTimeToCollision( +Accumulator calcTimeToCollision( const PredictedObjects & obstacles, const Trajectory & traj, const double distance_threshold) { - Stat stat; + Accumulator stat; /** TODO(Maxime CLEMENT): * this implementation assumes static obstacles and does not work for dynamic obstacles */ diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index 4d1c02faa406f..e6ede0d07b9b3 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -29,9 +29,9 @@ namespace metrics { using autoware_planning_msgs::msg::TrajectoryPoint; -Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2) +Accumulator calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2) { - Stat stat; + Accumulator stat; if (traj1.points.empty() || traj2.points.empty()) { return stat; @@ -58,9 +58,9 @@ Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & tr return stat; } -Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2) +Accumulator calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2) { - Stat stat; + Accumulator stat; if (traj1.points.empty()) { return stat; } diff --git a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp index 4526865ced97d..bd5eed06f96f8 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -23,18 +23,19 @@ namespace metrics using autoware::universe_utils::calcCurvature; using autoware::universe_utils::calcDistance2d; -Stat calcTrajectoryInterval(const Trajectory & traj) +Accumulator calcTrajectoryInterval(const Trajectory & traj) { - Stat stat; + Accumulator stat; for (size_t i = 1; i < traj.points.size(); ++i) { stat.add(calcDistance2d(traj.points.at(i), traj.points.at(i - 1))); } return stat; } -Stat calcTrajectoryRelativeAngle(const Trajectory & traj, const double min_dist_threshold) +Accumulator calcTrajectoryRelativeAngle( + const Trajectory & traj, const double min_dist_threshold) { - Stat stat; + Accumulator stat; // We need at least three points to compute relative angle const size_t relative_angle_points_num = 3; if (traj.points.size() < relative_angle_points_num) { @@ -79,9 +80,9 @@ Stat calcTrajectoryRelativeAngle(const Trajectory & traj, const double m return stat; } -Stat calcTrajectoryCurvature(const Trajectory & traj) +Accumulator calcTrajectoryCurvature(const Trajectory & traj) { - Stat stat; + Accumulator stat; // We need at least three points to compute curvature if (traj.points.size() < 3) { return stat; @@ -111,18 +112,18 @@ Stat calcTrajectoryCurvature(const Trajectory & traj) return stat; } -Stat calcTrajectoryLength(const Trajectory & traj) +Accumulator calcTrajectoryLength(const Trajectory & traj) { double length = 0.0; for (size_t i = 1; i < traj.points.size(); ++i) { length += calcDistance2d(traj.points.at(i), traj.points.at(i - 1)); } - Stat stat; + Accumulator stat; stat.add(length); return stat; } -Stat calcTrajectoryDuration(const Trajectory & traj) +Accumulator calcTrajectoryDuration(const Trajectory & traj) { double duration = 0.0; for (size_t i = 0; i + 1 < traj.points.size(); ++i) { @@ -132,32 +133,32 @@ Stat calcTrajectoryDuration(const Trajectory & traj) duration += length / std::abs(velocity); } } - Stat stat; + Accumulator stat; stat.add(duration); return stat; } -Stat calcTrajectoryVelocity(const Trajectory & traj) +Accumulator calcTrajectoryVelocity(const Trajectory & traj) { - Stat stat; + Accumulator stat; for (TrajectoryPoint p : traj.points) { stat.add(p.longitudinal_velocity_mps); } return stat; } -Stat calcTrajectoryAcceleration(const Trajectory & traj) +Accumulator calcTrajectoryAcceleration(const Trajectory & traj) { - Stat stat; + Accumulator stat; for (TrajectoryPoint p : traj.points) { stat.add(p.acceleration_mps2); } return stat; } -Stat calcTrajectoryJerk(const Trajectory & traj) +Accumulator calcTrajectoryJerk(const Trajectory & traj) { - Stat stat; + Accumulator stat; for (size_t i = 0; i + 1 < traj.points.size(); ++i) { const double vel = traj.points.at(i).longitudinal_velocity_mps; if (vel != 0) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 8658a666b4976..6e057bcc9fc3d 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -22,7 +22,7 @@ #include "autoware/universe_utils/geometry/geometry.hpp" namespace planning_diagnostics { -std::optional> MetricsCalculator::calculate( +std::optional> MetricsCalculator::calculate( const Metric metric, const Trajectory & traj) const { // Functions to calculate trajectory metrics @@ -74,7 +74,7 @@ std::optional> MetricsCalculator::calculate( } } -std::optional> MetricsCalculator::calculate( +std::optional> MetricsCalculator::calculate( const Metric metric, const Pose & base_pose, const Pose & target_pose) const { // Functions to calculate pose metrics diff --git a/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp index a5320ef28cbec..ac6f35179f771 100644 --- a/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp @@ -14,8 +14,11 @@ #include "autoware/planning_evaluator/motion_evaluator_node.hpp" +#include + +#include +#include #include -#include #include #include #include @@ -32,7 +35,7 @@ MotionEvaluatorNode::MotionEvaluatorNode(const rclcpp::NodeOptions & node_option "~/input/twist", rclcpp::QoS{1}, std::bind(&MotionEvaluatorNode::onOdom, this, std::placeholders::_1)); - output_file_str_ = declare_parameter("output_file"); + output_metrics_ = declare_parameter("output_metrics"); // List of metrics to calculate for (const std::string & selected_metric : @@ -44,22 +47,51 @@ MotionEvaluatorNode::MotionEvaluatorNode(const rclcpp::NodeOptions & node_option MotionEvaluatorNode::~MotionEvaluatorNode() { - // column width is the maximum size we might print + 1 for the space between columns - const auto column_width = 20; // std::to_string(std::numeric_limits::max()).size() + 1; - // Write data using format - std::ofstream f(output_file_str_); - f << std::fixed << std::left; - for (Metric metric : metrics_) { - f << std::setw(3 * column_width) << metric_descriptions.at(metric); + if (!output_metrics_) { + return; } - f << std::endl; + + // generate json data + using json = nlohmann::json; + json j; for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_); if (stat) { - f /* << std::setw(3 * column_width) */ << *stat << " "; + j[base_name + "min"] = stat->min(); + j[base_name + "max"] = stat->max(); + j[base_name + "mean"] = stat->mean(); } } - f.close(); + + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } + } + + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_motion_evaluator-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } } void MotionEvaluatorNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index e5e1c947a735c..a61e56cd532ad 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -16,13 +16,16 @@ #include #include +#include #include #include "boost/lexical_cast.hpp" +#include +#include #include -#include +#include #include #include #include @@ -50,7 +53,7 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op metrics_calculator_.parameters.obstacle.dist_thr_m = declare_parameter("obstacle.dist_thr_m"); - output_file_str_ = declare_parameter("output_file"); + output_metrics_ = declare_parameter("output_metrics"); ego_frame_str_ = declare_parameter("ego_frame"); processing_time_pub_ = @@ -67,34 +70,49 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op PlanningEvaluatorNode::~PlanningEvaluatorNode() { - if (!output_file_str_.empty()) { - // column width is the maximum size we might print + 1 for the space between columns - // Write data using format - std::ofstream f(output_file_str_); - f << std::fixed << std::left; - // header - f << "#Stamp(ns)"; - for (Metric metric : metrics_) { - f << " " << metric_descriptions.at(metric); - f << " . ."; // extra "columns" to align columns headers - } - f << std::endl; - f << "#."; - for (Metric metric : metrics_) { - (void)metric; - f << " min max mean"; - } - f << std::endl; - // data - for (size_t i = 0; i < stamps_.size(); ++i) { - f << stamps_[i].nanoseconds(); - for (Metric metric : metrics_) { - const auto & stat = metric_stats_[static_cast(metric)][i]; - f << " " << stat; - } - f << std::endl; + if (!output_metrics_) { + return; + } + + // generate json data + using json = nlohmann::json; + json j; + for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; + j[base_name + "min"] = metric_accumulators_[static_cast(metric)][0].min(); + j[base_name + "max"] = metric_accumulators_[static_cast(metric)][1].max(); + j[base_name + "mean"] = metric_accumulators_[static_cast(metric)][2].mean(); + j[base_name + "count"] = metric_accumulators_[static_cast(metric)][2].count(); + j[base_name + "description"] = metric_descriptions.at(metric); + } + + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; } + } + + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_planning_evaluator-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); } } @@ -198,7 +216,8 @@ void PlanningEvaluatorNode::AddKinematicStateMetricMsg( return; } -void PlanningEvaluatorNode::AddMetricMsg(const Metric & metric, const Stat & metric_stat) +void PlanningEvaluatorNode::AddMetricMsg( + const Metric & metric, const Accumulator & metric_stat) { const std::string base_name = metric_to_str.at(metric) + "/"; MetricMsg metric_msg; @@ -268,9 +287,6 @@ void PlanningEvaluatorNode::onTrajectory( } auto start = now(); - if (!output_file_str_.empty()) { - stamps_.push_back(traj_msg->header.stamp); - } for (Metric metric : metrics_) { const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg); @@ -278,8 +294,10 @@ void PlanningEvaluatorNode::onTrajectory( continue; } - if (!output_file_str_.empty()) { - metric_stats_[static_cast(metric)].push_back(*metric_stat); + if (output_metrics_) { + metric_accumulators_[static_cast(metric)][0].add(metric_stat->min()); + metric_accumulators_[static_cast(metric)][1].add(metric_stat->max()); + metric_accumulators_[static_cast(metric)][2].add(metric_stat->mean()); } if (metric_stat->count() > 0) { @@ -307,7 +325,11 @@ void PlanningEvaluatorNode::onModifiedGoal( if (!metric_stat) { continue; } - metric_stats_[static_cast(metric)].push_back(*metric_stat); + if (output_metrics_) { + metric_accumulators_[static_cast(metric)][0].add(metric_stat->min()); + metric_accumulators_[static_cast(metric)][1].add(metric_stat->max()); + metric_accumulators_[static_cast(metric)][2].add(metric_stat->mean()); + } if (metric_stat->count() > 0) { AddMetricMsg(metric, *metric_stat); } diff --git a/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp index ac99d164708ba..f202bb4ab0e6f 100644 --- a/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp @@ -27,6 +27,7 @@ #include "boost/lexical_cast.hpp" +#include #include #include #include @@ -53,7 +54,8 @@ class EvalTest : public ::testing::Test const auto share_dir = ament_index_cpp::get_package_share_directory("autoware_planning_evaluator"); options.arguments( - {"--ros-args", "--params-file", share_dir + "/config/planning_evaluator.param.yaml"}); + {"--ros-args", "--params-file", share_dir + "/config/planning_evaluator.param.yaml", "-p", + "output_metrics:=false"}); dummy_node = std::make_shared("planning_evaluator_test_node"); eval_node = std::make_shared(options); diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp index 16b4230fc4656..d401f63649468 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp +++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp @@ -15,8 +15,8 @@ #ifndef KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ #define KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ +#include "autoware/universe_utils/math/accumulator.hpp" #include "kinematic_evaluator/metrics_calculator.hpp" -#include "kinematic_evaluator/stat.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -33,6 +33,7 @@ namespace kinematic_diagnostics { +using autoware::universe_utils::Accumulator; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; @@ -55,7 +56,7 @@ class KinematicEvaluatorNode : public rclcpp::Node * @brief publish the given metric statistic */ DiagnosticStatus generateDiagnosticStatus( - const Metric & metric, const Stat & metric_stat) const; + const Metric & metric, const Accumulator & metric_stat) const; private: geometry_msgs::msg::Pose getCurrentEgoPose() const; @@ -74,8 +75,8 @@ class KinematicEvaluatorNode : public rclcpp::Node // Metrics std::vector metrics_; std::deque stamps_; - std::array>, static_cast(Metric::SIZE)> metric_stats_; - std::unordered_map> metrics_dict_; + std::array>, static_cast(Metric::SIZE)> metric_stats_; + std::unordered_map> metrics_dict_; }; } // namespace kinematic_diagnostics diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp index d20bf079a363a..e7d18910e7c39 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp +++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp @@ -15,7 +15,7 @@ #ifndef KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ #define KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ -#include "kinematic_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include @@ -23,6 +23,7 @@ namespace kinematic_diagnostics { namespace metrics { +using autoware::universe_utils::Accumulator; using nav_msgs::msg::Odometry; /** @@ -31,7 +32,7 @@ using nav_msgs::msg::Odometry; * @param [in] stat_prev input trajectory * @return calculated statistics */ -Stat updateVelocityStats(const double & value, const Stat stat_prev); +Accumulator updateVelocityStats(const double & value, const Accumulator stat_prev); } // namespace metrics } // namespace kinematic_diagnostics diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp index 67f152e30c9eb..372d4a34af62c 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp +++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp @@ -15,15 +15,16 @@ #ifndef KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_ #define KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_ +#include "autoware/universe_utils/math/accumulator.hpp" #include "kinematic_evaluator/metrics/metric.hpp" #include "kinematic_evaluator/parameters.hpp" -#include "kinematic_evaluator/stat.hpp" #include "geometry_msgs/msg/pose.hpp" #include namespace kinematic_diagnostics { +using autoware::universe_utils::Accumulator; using nav_msgs::msg::Odometry; class MetricsCalculator @@ -39,7 +40,7 @@ class MetricsCalculator * @param [in] odom Odometry * @return string describing the requested metric */ - Stat calculate(const Metric metric, const Odometry & odom) const; + Accumulator calculate(const Metric metric, const Odometry & odom) const; /** * @brief update Metrics @@ -47,8 +48,8 @@ class MetricsCalculator * @param [in] odom Odometry * @return string describing the requested metric */ - Stat updateStat( - const Metric metric, const Odometry & odom, const Stat stat_prev) const; + Accumulator updateStat( + const Metric metric, const Odometry & odom, const Accumulator stat_prev) const; }; // class MetricsCalculator } // namespace kinematic_diagnostics diff --git a/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp b/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp index 57631e5f861df..d34f9deb7f1ba 100644 --- a/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp +++ b/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp @@ -46,7 +46,7 @@ KinematicEvaluatorNode::KinematicEvaluatorNode(const rclcpp::NodeOptions & node_ for (const std::string & selected_metric : declare_parameter>("selected_metrics")) { Metric metric = str_to_metric.at(selected_metric); - metrics_dict_[metric] = Stat(); + metrics_dict_[metric] = Accumulator(); metrics_.push_back(metric); } } @@ -83,7 +83,7 @@ KinematicEvaluatorNode::~KinematicEvaluatorNode() } DiagnosticStatus KinematicEvaluatorNode::generateDiagnosticStatus( - const Metric & metric, const Stat & metric_stat) const + const Metric & metric, const Accumulator & metric_stat) const { DiagnosticStatus status; status.level = status.OK; diff --git a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp index 2bb0dad86bf71..d410863a05010 100644 --- a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp +++ b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp @@ -19,9 +19,9 @@ namespace kinematic_diagnostics namespace metrics { -Stat updateVelocityStats(const double & value, const Stat stat_prev) +Accumulator updateVelocityStats(const double & value, const Accumulator stat_prev) { - Stat stat(stat_prev); + Accumulator stat(stat_prev); stat.add(value); return stat; } diff --git a/evaluator/kinematic_evaluator/src/metrics_calculator.cpp b/evaluator/kinematic_evaluator/src/metrics_calculator.cpp index af1e1114b9c43..8b0f581817b38 100644 --- a/evaluator/kinematic_evaluator/src/metrics_calculator.cpp +++ b/evaluator/kinematic_evaluator/src/metrics_calculator.cpp @@ -18,8 +18,8 @@ namespace kinematic_diagnostics { -Stat MetricsCalculator::updateStat( - const Metric metric, const Odometry & odom, const Stat stat_prev) const +Accumulator MetricsCalculator::updateStat( + const Metric metric, const Odometry & odom, const Accumulator stat_prev) const { switch (metric) { case Metric::velocity_stats: diff --git a/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp b/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp index 51e6113aea999..769f98416848f 100644 --- a/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp +++ b/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp @@ -26,6 +26,7 @@ #include "boost/lexical_cast.hpp" +#include #include #include #include diff --git a/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp b/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp index 8c93ddd5fa0fc..feb61dd3cacbe 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp +++ b/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp @@ -15,8 +15,8 @@ #ifndef LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ #define LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ +#include "autoware/universe_utils/math/accumulator.hpp" #include "localization_evaluator/metrics_calculator.hpp" -#include "localization_evaluator/stat.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -38,6 +38,7 @@ namespace localization_diagnostics { +using autoware::universe_utils::Accumulator; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using geometry_msgs::msg::PoseWithCovarianceStamped; @@ -70,7 +71,7 @@ class LocalizationEvaluatorNode : public rclcpp::Node * @brief publish the given metric statistic */ DiagnosticStatus generateDiagnosticStatus( - const Metric & metric, const Stat & metric_stat) const; + const Metric & metric, const Accumulator & metric_stat) const; private: // ROS @@ -93,8 +94,8 @@ class LocalizationEvaluatorNode : public rclcpp::Node // Metrics std::vector metrics_; std::deque stamps_; - std::array>, static_cast(Metric::SIZE)> metric_stats_; - std::unordered_map> metrics_dict_; + std::array>, static_cast(Metric::SIZE)> metric_stats_; + std::unordered_map> metrics_dict_; }; } // namespace localization_diagnostics diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp b/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp index c973d285fd503..82119efca6082 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp +++ b/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp @@ -15,12 +15,13 @@ #ifndef LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ #define LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ -#include "localization_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include namespace localization_diagnostics { +using autoware::universe_utils::Accumulator; namespace metrics { /** @@ -30,8 +31,8 @@ namespace metrics * @param [in] lateral_ref reference lateral position * @return calculated statistics */ -Stat updateLateralStats( - const Stat stat_prev, const double & lateral_pos, const double & lateral_ref); +Accumulator updateLateralStats( + const Accumulator stat_prev, const double & lateral_pos, const double & lateral_ref); /** * @brief calculate absolute localization error @@ -40,8 +41,8 @@ Stat updateLateralStats( * @param [in] pos_ref reference position of the vehicle * @return calculated statistics */ -Stat updateAbsoluteStats( - const Stat stat_prev, const geometry_msgs::msg::Point & pos, +Accumulator updateAbsoluteStats( + const Accumulator stat_prev, const geometry_msgs::msg::Point & pos, const geometry_msgs::msg::Point & pos_ref); } // namespace metrics diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp b/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp index 88f8ea85d4267..310d1b25e92f1 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp +++ b/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp @@ -15,15 +15,16 @@ #ifndef LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_ #define LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_ +#include "autoware/universe_utils/math/accumulator.hpp" #include "localization_evaluator/metrics/metric.hpp" #include "localization_evaluator/parameters.hpp" -#include "localization_evaluator/stat.hpp" #include "geometry_msgs/msg/pose.hpp" #include namespace localization_diagnostics { +using autoware::universe_utils::Accumulator; class MetricsCalculator { public: @@ -38,8 +39,8 @@ class MetricsCalculator * @param [in] pos_ref reference position * @return string describing the requested metric */ - Stat updateStat( - const Stat stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos, + Accumulator updateStat( + const Accumulator stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos, const geometry_msgs::msg::Point & pos_ref) const; }; // class MetricsCalculator diff --git a/evaluator/localization_evaluator/include/localization_evaluator/stat.hpp b/evaluator/localization_evaluator/include/localization_evaluator/stat.hpp deleted file mode 100644 index fbea5b61813fb..0000000000000 --- a/evaluator/localization_evaluator/include/localization_evaluator/stat.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#ifndef LOCALIZATION_EVALUATOR__STAT_HPP_ -#define LOCALIZATION_EVALUATOR__STAT_HPP_ - -namespace localization_diagnostics -{ -/** - * @brief class to incrementally build statistics - * @typedef T type of the values (default to double) - */ -template -class Stat -{ -public: - /** - * @brief add a value - * @param value value to add - */ - void add(const T & value) - { - if (value < min_) { - min_ = value; - } - if (value > max_) { - max_ = value; - } - ++count_; - mean_ = mean_ + (value - mean_) / count_; - } - - /** - * @brief get the mean value - */ - long double mean() const { return mean_; } - - /** - * @brief get the minimum value - */ - T min() const { return min_; } - - /** - * @brief get the maximum value - */ - T max() const { return max_; } - - /** - * @brief get the number of values used to build this statistic - */ - unsigned int count() const { return count_; } - - template - friend std::ostream & operator<<(std::ostream & os, const Stat & stat); - -private: - T min_ = std::numeric_limits::max(); - T max_ = std::numeric_limits::min(); - long double mean_ = 0.0; - unsigned int count_ = 0; -}; - -/** - * @brief overload << operator for easy print to output stream - */ -template -std::ostream & operator<<(std::ostream & os, const Stat & stat) -{ - if (stat.count() == 0) { - os << "None None None"; - } else { - os << stat.min() << " " << stat.max() << " " << stat.mean(); - } - return os; -} - -} // namespace localization_diagnostics - -#endif // LOCALIZATION_EVALUATOR__STAT_HPP_ diff --git a/evaluator/localization_evaluator/package.xml b/evaluator/localization_evaluator/package.xml index 49dd156f983b4..b9d6285a0f08c 100644 --- a/evaluator/localization_evaluator/package.xml +++ b/evaluator/localization_evaluator/package.xml @@ -6,6 +6,11 @@ Dominik Jargot Koji Minoda + Anh Nguyen + Masahiro Sakamoto + Shintaro Sakoda + Taiki Yamada + Yamato Ando Apache License 2.0 diff --git a/evaluator/localization_evaluator/src/localization_evaluator_node.cpp b/evaluator/localization_evaluator/src/localization_evaluator_node.cpp index db0c04a698725..8a9352d2a73e8 100644 --- a/evaluator/localization_evaluator/src/localization_evaluator_node.cpp +++ b/evaluator/localization_evaluator/src/localization_evaluator_node.cpp @@ -49,7 +49,7 @@ LocalizationEvaluatorNode::LocalizationEvaluatorNode(const rclcpp::NodeOptions & for (const std::string & selected_metric : declare_parameter>("selected_metrics")) { Metric metric = str_to_metric.at(selected_metric); - metrics_dict_[metric] = Stat(); + metrics_dict_[metric] = Accumulator(); metrics_.push_back(metric); } } @@ -86,7 +86,7 @@ LocalizationEvaluatorNode::~LocalizationEvaluatorNode() } DiagnosticStatus LocalizationEvaluatorNode::generateDiagnosticStatus( - const Metric & metric, const Stat & metric_stat) const + const Metric & metric, const Accumulator & metric_stat) const { DiagnosticStatus status; status.level = status.OK; diff --git a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp index fab3377913bd4..97fd8e326cca7 100644 --- a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp +++ b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp @@ -21,19 +21,19 @@ namespace localization_diagnostics namespace metrics { -Stat updateLateralStats( - const Stat stat_prev, const double & lateral_pos, const double & lateral_ref) +Accumulator updateLateralStats( + const Accumulator stat_prev, const double & lateral_pos, const double & lateral_ref) { - Stat stat(stat_prev); + Accumulator stat(stat_prev); stat.add(std::abs(lateral_ref - lateral_pos)); return stat; } -Stat updateAbsoluteStats( - const Stat stat_prev, const geometry_msgs::msg::Point & pos, +Accumulator updateAbsoluteStats( + const Accumulator stat_prev, const geometry_msgs::msg::Point & pos, const geometry_msgs::msg::Point & pos_ref) { - Stat stat(stat_prev); + Accumulator stat(stat_prev); double dist = std::sqrt( (pos_ref.x - pos.x) * (pos_ref.x - pos.x) + (pos_ref.y - pos.y) * (pos_ref.y - pos.y) + (pos_ref.z - pos.z) * (pos_ref.z - pos.z)); diff --git a/evaluator/localization_evaluator/src/metrics_calculator.cpp b/evaluator/localization_evaluator/src/metrics_calculator.cpp index 72184937e846b..864fb4a5ddbd2 100644 --- a/evaluator/localization_evaluator/src/metrics_calculator.cpp +++ b/evaluator/localization_evaluator/src/metrics_calculator.cpp @@ -18,8 +18,8 @@ namespace localization_diagnostics { -Stat MetricsCalculator::updateStat( - const Stat stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos, +Accumulator MetricsCalculator::updateStat( + const Accumulator stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos, const geometry_msgs::msg::Point & pos_ref) const { if ( diff --git a/evaluator/localization_evaluator/test/test_localization_evaluator_node.cpp b/evaluator/localization_evaluator/test/test_localization_evaluator_node.cpp index cd6a7b80fd35b..6ab428c6fb578 100644 --- a/evaluator/localization_evaluator/test/test_localization_evaluator_node.cpp +++ b/evaluator/localization_evaluator/test/test_localization_evaluator_node.cpp @@ -27,6 +27,7 @@ #include "boost/lexical_cast.hpp" +#include #include #include #include diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md index 7df375ac236d0..17e7e1a2b5652 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_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` | `tier4_metric_msgs::msg::MetricArray` | Metric 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 010f1497da3da..c111a725a2ea0 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 @@ -16,7 +16,6 @@ #define PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_ #include "perception_online_evaluator/parameters.hpp" -#include "perception_online_evaluator/stat.hpp" #include "tf2_ros/buffer.h" #include 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 d67413c174c41..0f3379b3f055e 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 @@ -15,8 +15,6 @@ #ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ #define PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#include "perception_online_evaluator/stat.hpp" - #include #include diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index eaa07f2317940..4caf932919e61 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -15,7 +15,7 @@ #ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ #define PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ -#include "perception_online_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include #include @@ -39,14 +39,15 @@ enum class Metric { // Each metric has a different return type. (statistic or just a one value etc). // To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant -using MetricStatMap = std::unordered_map>; +using autoware::universe_utils::Accumulator; +using MetricStatMap = std::unordered_map>; using MetricValueMap = std::unordered_map; using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { - Stat mean; - Stat variance; + Accumulator mean; + Accumulator variance; }; static const std::unordered_map str_to_metric = { 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 a9b1388281ce8..c17051d2a30a7 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 @@ -19,7 +19,6 @@ #include "perception_online_evaluator/metrics/deviation_metrics.hpp" #include "perception_online_evaluator/metrics/metric.hpp" #include "perception_online_evaluator/parameters.hpp" -#include "perception_online_evaluator/stat.hpp" #include "perception_online_evaluator/utils/objects_filtering.hpp" #include "tf2_ros/buffer.h" 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 1bc427e667a2a..61c1ba40134df 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 @@ -15,17 +15,18 @@ #ifndef PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ #define PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ +#include "autoware/universe_utils/math/accumulator.hpp" #include "perception_online_evaluator/metrics_calculator.hpp" #include "perception_online_evaluator/parameters.hpp" -#include "perception_online_evaluator/stat.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #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" +#include +#include #include #include @@ -35,10 +36,9 @@ namespace perception_diagnostics { +using autoware::universe_utils::Accumulator; 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; using TFMessage = tf2_msgs::msg::TFMessage; @@ -59,15 +59,34 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node */ void onObjects(const PredictedObjects::ConstSharedPtr objects_msg); - DiagnosticStatus generateDiagnosticStatus( - const std::string metric, const Stat & metric_stat) const; - DiagnosticStatus generateDiagnosticStatus( - const std::string & metric, const double metric_value) const; + /** + * @brief Convert metric statistic to `tier4_metric_msgs::msg::Metric` and append to + * `tier4_metric_msgs::msg::MetricArray`. + * + * @param metric Metric name. + * @param metric_stat Metric statistic. + * @param metrics_msg Metrics value container. + */ + void toMetricMsg( + const std::string & metric, const Accumulator & metric_stat, + tier4_metric_msgs::msg::MetricArray & metrics_msg) const; + + /** + * @brief Convert metric value to `tier4_metric_msgs::msg::Metric` and append to + * `tier4_metric_msgs::msg::MetricArray + * + * @param metric Metric name. + * @param metric_stat Metric value. + * @param metrics_msg Metrics value container. + */ + void toMetricMsg( + const std::string & metric, const double metric_value, + tier4_metric_msgs::msg::MetricArray & metrics_msg) const; private: // Subscribers and publishers rclcpp::Subscription::SharedPtr objects_sub_; - rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr metrics_pub_; rclcpp::Publisher::SharedPtr pub_marker_; // TF diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp deleted file mode 100644 index 63494f90d02ee..0000000000000 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#ifndef PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ -#define PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ - -namespace perception_diagnostics -{ -/** - * @brief class to incrementally build statistics - * @typedef T type of the values (default to double) - */ -template -class Stat -{ -public: - /** - * @brief add a value - * @param value value to add - */ - void add(const T & value) - { - if (value < min_) { - min_ = value; - } - if (value > max_) { - max_ = value; - } - ++count_; - mean_ = mean_ + (value - mean_) / count_; - } - - /** - * @brief get the mean value - */ - long double mean() const { return mean_; } - - /** - * @brief get the minimum value - */ - T min() const { return min_; } - - /** - * @brief get the maximum value - */ - T max() const { return max_; } - - /** - * @brief get the number of values used to build this statistic - */ - unsigned int count() const { return count_; } - - template - friend std::ostream & operator<<(std::ostream & os, const Stat & stat); - -private: - T min_ = std::numeric_limits::max(); - T max_ = std::numeric_limits::min(); - long double mean_ = 0.0; - unsigned int count_ = 0; -}; - -/** - * @brief overload << operator for easy print to output stream - */ -template -std::ostream & operator<<(std::ostream & os, const Stat & stat) -{ - if (stat.count() == 0) { - os << "None None None"; - } else { - os << stat.min() << " " << stat.max() << " " << stat.mean(); - } - return os; -} - -} // namespace perception_diagnostics - -#endif // PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index 654586a76453d..1e7c0a7d128e6 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -23,7 +23,6 @@ autoware_perception_msgs autoware_universe_utils autoware_vehicle_info_utils - diagnostic_msgs eigen geometry_msgs glog @@ -33,6 +32,7 @@ rclcpp_components tf2 tf2_ros + tier4_metric_msgs ament_cmake_ros ament_index_cpp diff --git a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp index ab69ea9cb8c7b..4a1d97aeeb90b 100644 --- a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp @@ -20,6 +20,11 @@ #include +#include +#include +#include +#include + namespace perception_diagnostics { namespace metrics diff --git a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp index 8570cf246f430..292fc9cd65a17 100644 --- a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp @@ -19,6 +19,8 @@ #include +#include + namespace perception_diagnostics { namespace metrics diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index fea4316fa6820..0ced0b9d6c8a8 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -235,7 +235,7 @@ MetricStatMap MetricsCalculator::calcLateralDeviationMetrics( !parameters_->object_parameters.at(label).check_lateral_deviation) { continue; } - Stat stat{}; + Accumulator stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { const auto uuid = autoware::universe_utils::toHexString(object.object_id); @@ -264,7 +264,7 @@ MetricStatMap MetricsCalculator::calcYawDeviationMetrics( !parameters_->object_parameters.at(label).check_yaw_deviation) { continue; } - Stat stat{}; + Accumulator stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { const auto uuid = autoware::universe_utils::toHexString(object.object_id); @@ -420,7 +420,7 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas MetricStatMap metric_stat_map{}; for (const auto & [label, objects] : class_objects_map) { - Stat stat{}; + Accumulator stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index a577fd359563c..7d8344c24819c 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -50,7 +50,7 @@ PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode( objects_sub_ = create_subscription( "~/input/objects", 1, std::bind(&PerceptionOnlineEvaluatorNode::onObjects, this, _1)); - metrics_pub_ = create_publisher("~/metrics", 1); + metrics_pub_ = create_publisher("~/metrics", 1); pub_marker_ = create_publisher("~/markers", 10); tf_buffer_ = std::make_unique(this->get_clock()); @@ -65,7 +65,8 @@ PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode( void PerceptionOnlineEvaluatorNode::publishMetrics() { - DiagnosticArray metrics_msg; + // DiagnosticArray metrics_msg; + tier4_metric_msgs::msg::MetricArray metrics_msg; // calculate metrics for (const Metric & metric : parameters_->metrics) { @@ -80,10 +81,10 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() for (const auto & [metric, value] : arg) { if constexpr (std::is_same_v) { if (value.count() > 0) { - metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + toMetricMsg(metric, value, metrics_msg); } } else if constexpr (std::is_same_v) { - metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + toMetricMsg(metric, value, metrics_msg); } } }, @@ -91,49 +92,44 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() } // publish metrics - if (!metrics_msg.status.empty()) { - metrics_msg.header.stamp = now(); + if (!metrics_msg.metric_array.empty()) { + metrics_msg.stamp = now(); metrics_pub_->publish(metrics_msg); } publishDebugMarker(); } -DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( - const std::string metric, const Stat & metric_stat) const +void PerceptionOnlineEvaluatorNode::toMetricMsg( + const std::string & metric, const Accumulator & metric_stat, + tier4_metric_msgs::msg::MetricArray & metrics_msg) const { - DiagnosticStatus status; - - status.level = status.OK; - status.name = metric; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "min"; - key_value.value = std::to_string(metric_stat.min()); - status.values.push_back(key_value); - key_value.key = "max"; - key_value.value = std::to_string(metric_stat.max()); - status.values.push_back(key_value); - key_value.key = "mean"; - key_value.value = std::to_string(metric_stat.mean()); - status.values.push_back(key_value); - - return status; + // min value + metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build() + .name(metric + "/min") + .unit("") + .value(std::to_string(metric_stat.min()))); + + // max value + metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build() + .name(metric + "/max") + .unit("") + .value(std::to_string(metric_stat.max()))); + + // mean value + metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build() + .name(metric + "/mean") + .unit("") + .value(std::to_string(metric_stat.mean()))); } -DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( - const std::string & metric, const double value) const +void PerceptionOnlineEvaluatorNode::toMetricMsg( + const std::string & metric, const double metric_value, + tier4_metric_msgs::msg::MetricArray & metrics_msg) const { - DiagnosticStatus status; - - status.level = status.OK; - status.name = metric; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "metric_value"; - key_value.value = std::to_string(value); - status.values.push_back(key_value); - - return status; + metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build() + .name(metric + "/metric_value") + .unit("") + .value(std::to_string(metric_value))); } void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp index 0c6d1cac4246e..7489447ccb47e 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -24,7 +24,10 @@ #include #include +#include #include +#include +#include namespace marker_utils { diff --git a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp index 6ffb80eb40b59..13bd820f18cec 100644 --- a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp +++ b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp @@ -14,6 +14,9 @@ #include "perception_online_evaluator/utils/objects_filtering.hpp" +#include +#include + namespace perception_diagnostics { namespace filter 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 e2ab22be2b61b..ffefee835f933 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 @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include "boost/lexical_cast.hpp" @@ -29,6 +29,8 @@ #include #include +#include +#include #include #include #include @@ -37,7 +39,6 @@ using EvalNode = perception_diagnostics::PerceptionOnlineEvaluatorNode; 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_perception_msgs::msg::ObjectClassification; using nav_msgs::msg::Odometry; @@ -125,36 +126,29 @@ class EvalTest : public ::testing::Test tf_pub_->publish(tf_msg); } - void setTargetMetric(perception_diagnostics::Metric metric) + void setTargetMetric(const perception_diagnostics::Metric & metric) { const auto metric_str = perception_diagnostics::metric_to_str.at(metric); setTargetMetric(metric_str); } - void setTargetMetric(std::string metric_str) + void setTargetMetric(const std::string & metric_str) { - const auto is_target_metric = [metric_str](const auto & status) { - return status.name == metric_str; - }; - metric_sub_ = rclcpp::create_subscription( + metric_sub_ = rclcpp::create_subscription( eval_node, "/perception_online_evaluator/metrics", 1, - [=](const DiagnosticArray::ConstSharedPtr msg) { - const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); - if (it != msg->status.end()) { - const auto mean_it = std::find_if( - it->values.begin(), it->values.end(), - [](const auto & key_value) { return key_value.key == "mean"; }); - if (mean_it != it->values.end()) { - metric_value_ = boost::lexical_cast(mean_it->value); - } else { - const auto metric_value_it = std::find_if( - it->values.begin(), it->values.end(), - [](const auto & key_value) { return key_value.key == "metric_value"; }); - if (metric_value_it != it->values.end()) { - metric_value_ = boost::lexical_cast(metric_value_it->value); - } - } + [this, metric_str](const tier4_metric_msgs::msg::MetricArray::ConstSharedPtr msg) { + // extract a metric whose name includes metrics_str + const auto it = std::find_if( + msg->metric_array.begin(), msg->metric_array.end(), [&metric_str](const auto & metric) { + return metric.name == metric_str + "/metric_value" || + metric.name == metric_str + "/mean"; + }); + + if (it != msg->metric_array.end()) { + metric_value_ = boost::lexical_cast(it->value); metric_updated_ = true; + } else { + metric_updated_ = false; } }); } @@ -316,7 +310,7 @@ class EvalTest : public ::testing::Test // Pub/Sub rclcpp::Publisher::SharedPtr objects_pub_; - rclcpp::Subscription::SharedPtr metric_sub_; + rclcpp::Subscription::SharedPtr metric_sub_; rclcpp::Subscription::SharedPtr marker_sub_; rclcpp::Publisher::SharedPtr tf_pub_; bool has_received_marker_{false}; diff --git a/evaluator/scenario_simulator_v2_adapter/CHANGELOG.rst b/evaluator/scenario_simulator_v2_adapter/CHANGELOG.rst deleted file mode 100644 index 0c41ad6263d8b..0000000000000 --- a/evaluator/scenario_simulator_v2_adapter/CHANGELOG.rst +++ /dev/null @@ -1,80 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package diagnostic_converter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* Contributors: Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- -* fix(diagnostic_converter): move headers to a separate directory (`#5943 `_) - * fix(diagnostic_converter): move headers to a separate directory - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* build: mark autoware_cmake as (`#3616 `_) - * build: mark autoware_cmake as - with , autoware_cmake is automatically exported with ament_target_dependencies() (unecessary) - * style(pre-commit): autofix - * chore: fix pre-commit errors - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> - Co-authored-by: Kenji Miyake -* feat(diagnostic_converter): remove unit and blank in the value (`#3151 `_) - * feat(diagnostic_converter): remove unit and blank in the value - * fix - --------- -* feat(diagnostic_converter): apply regex for topic name (`#3149 `_) -* feat(diagnostic_converter): add converter to use planning_evaluator's output for scenario's condition (`#2514 `_) - * add original diagnostic_convertor - * add test - * fix typo - * delete file - * change include - * temp - * delete comments - * made launch for converter - * ci(pre-commit): autofix - * ci(pre-commit): autofix - * add diagnostic convertor in launch - * ci(pre-commit): autofix - * change debug from info - * change arg name to launch diagnostic convertor - * add planning_evaluator launcher in simulator.launch.xml - * fix arg wrong setting - * style(pre-commit): autofix - * use simulation msg in tier4_autoware_msgs - * style(pre-commit): autofix - * fix README - * style(pre-commit): autofix - * refactoring - * style(pre-commit): autofix - * remove unnecessary dependency - * remove unnecessary dependency - * move folder - * reformat - * style(pre-commit): autofix - * Update evaluator/diagnostic_converter/include/converter_node.hpp - Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> - * Update evaluator/diagnostic_converter/README.md - Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> - * Update evaluator/diagnostic_converter/src/converter_node.cpp - Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> - * Update evaluator/diagnostic_converter/test/test_converter_node.cpp - Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> - * define diagnostic_topics as parameter - * fix include way - * fix include way - * delete ament_cmake_clang_format from package.xml - * fix test_depend - * Update evaluator/diagnostic_converter/test/test_converter_node.cpp - Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> - * style(pre-commit): autofix - * Update launch/tier4_simulator_launch/launch/simulator.launch.xml - Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> - Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> -* Contributors: Esteve Fernandez, Kyoichi Sugahara, Takayuki Murooka, Vincent Richard diff --git a/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp b/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp index 98d36327793fe..4c0464b8b2d41 100644 --- a/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp +++ b/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp @@ -15,6 +15,8 @@ #include "scenario_simulator_v2_adapter/converter_node.hpp" #include +#include +#include namespace { diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index 899db1448dbf8..a979f9e28244b 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -51,7 +51,7 @@ - + @@ -177,7 +177,7 @@ - + @@ -269,21 +269,11 @@ + - - - - - - - - - - - - - - + + + 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 d88601497096d..41326c1a16728 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 @@ -233,6 +233,7 @@ + @@ -254,7 +255,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 b303134ab80c3..fb7e11419fdc9 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 @@ -27,7 +27,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index e0fdcb1e30408..89961821fa761 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -32,7 +32,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 9082ebab013ed..a89b47da13049 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 @@ -44,7 +44,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index f0fe45a46d92a..7f181d6d707b6 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -63,6 +63,7 @@ autoware_external_cmd_selector autoware_external_velocity_limit_selector autoware_freespace_planner + autoware_glog_component autoware_mission_planner autoware_obstacle_cruise_planner autoware_obstacle_stop_planner @@ -74,7 +75,6 @@ autoware_scenario_selector autoware_surround_obstacle_checker autoware_velocity_smoother - glog_component ament_lint_auto autoware_lint_common diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index e1226ee63f5e2..4b2cefa02c2fa 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -188,9 +188,9 @@ - + - + diff --git a/localization/autoware_ekf_localizer/README.md b/localization/autoware_ekf_localizer/README.md index fb367e5b42286..aad65da2516d1 100644 --- a/localization/autoware_ekf_localizer/README.md +++ b/localization/autoware_ekf_localizer/README.md @@ -54,6 +54,7 @@ This package includes the following features: | `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | | `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | | `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | +| `debug/processing_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | The processing time [ms]. | ### Published TF diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp index d61faa39a93a0..1b437f26e9c7d 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp @@ -85,6 +85,8 @@ class EKFLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_biased_pose_cov_; //!< @brief diagnostics publisher rclcpp::Publisher::SharedPtr pub_diag_; + //!< @brief processing_time publisher + rclcpp::Publisher::SharedPtr pub_processing_time_; //!< @brief initial pose subscriber rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief measurement pose with covariance subscriber @@ -185,6 +187,7 @@ class EKFLocalizer : public rclcpp::Node std_srvs::srv::SetBool::Response::SharedPtr res); autoware::universe_utils::StopWatch stop_watch_; + autoware::universe_utils::StopWatch stop_watch_timer_cb_; friend class EKFLocalizerTestSuite; // for test code }; diff --git a/localization/autoware_ekf_localizer/launch/ekf_localizer.launch.xml b/localization/autoware_ekf_localizer/launch/ekf_localizer.launch.xml index 2942abe1ff11e..76ac35bcba38e 100644 --- a/localization/autoware_ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/autoware_ekf_localizer/launch/ekf_localizer.launch.xml @@ -16,6 +16,7 @@ + @@ -32,6 +33,7 @@ + diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp index 70ae4c04436ec..d34be2a537ef1 100644 --- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp +++ b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp @@ -33,6 +33,7 @@ #include #include #include +#include namespace autoware::ekf_localizer { @@ -73,6 +74,8 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) pub_biased_pose_cov_ = create_publisher( "ekf_biased_pose_with_covariance", 1); pub_diag_ = this->create_publisher("/diagnostics", 10); + pub_processing_time_ = + create_publisher("debug/processing_time_ms", 1); sub_initialpose_ = create_subscription( "initialpose", 1, std::bind(&EKFLocalizer::callback_initial_pose, this, _1)); sub_pose_with_cov_ = create_subscription( @@ -129,6 +132,8 @@ void EKFLocalizer::update_predict_frequency(const rclcpp::Time & current_time) */ void EKFLocalizer::timer_callback() { + stop_watch_timer_cb_.tic(); + const rclcpp::Time current_time = this->now(); if (!is_activated_) { @@ -219,6 +224,12 @@ void EKFLocalizer::timer_callback() /* publish ekf result */ publish_estimate_result(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); publish_diagnostics(current_ekf_pose, current_time); + + /* publish processing time */ + const double elapsed_time = stop_watch_timer_cb_.toc(); + pub_processing_time_->publish(tier4_debug_msgs::build() + .stamp(current_time) + .data(elapsed_time)); } /* diff --git a/localization/autoware_ekf_localizer/test/test_aged_object_queue.cpp b/localization/autoware_ekf_localizer/test/test_aged_object_queue.cpp index 9cf56b80bb79a..dc9ef008335ed 100644 --- a/localization/autoware_ekf_localizer/test/test_aged_object_queue.cpp +++ b/localization/autoware_ekf_localizer/test/test_aged_object_queue.cpp @@ -16,6 +16,8 @@ #include +#include + namespace autoware::ekf_localizer { diff --git a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp index 979f4c75deb84..165102adec1d7 100644 --- a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp +++ b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp @@ -16,6 +16,9 @@ #include +#include +#include + namespace autoware::ekf_localizer { diff --git a/localization/autoware_ekf_localizer/test/test_numeric.cpp b/localization/autoware_ekf_localizer/test/test_numeric.cpp index 8071804f48304..080ce01f31770 100644 --- a/localization/autoware_ekf_localizer/test/test_numeric.cpp +++ b/localization/autoware_ekf_localizer/test/test_numeric.cpp @@ -18,6 +18,8 @@ #include +#include + namespace autoware::ekf_localizer { diff --git a/localization/autoware_geo_pose_projector/README.md b/localization/autoware_geo_pose_projector/README.md index 2dd83a2077aab..a3cc5d3b308e3 100644 --- a/localization/autoware_geo_pose_projector/README.md +++ b/localization/autoware_geo_pose_projector/README.md @@ -9,7 +9,7 @@ This node is a simple node that subscribes to the geo-referenced pose topic and | Name | Type | Description | | ------------------------- | ---------------------------------------------------- | ------------------- | | `input_geo_pose` | `geographic_msgs::msg::GeoPoseWithCovarianceStamped` | geo-referenced pose | -| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectedObjectInfo` | map projector info | +| `/map/map_projector_info` | `autoware_map_msgs::msg::MapProjectedObjectInfo` | map projector info | ## Published Topics diff --git a/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp b/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp index c3bf074f73aeb..0db808257f639 100644 --- a/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp +++ b/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp @@ -17,6 +17,8 @@ #include #include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 2418cf3f5dd1c..9d2a8bf814694 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -51,6 +51,10 @@ #include #include +#include +#include +#include + #if __has_include() #include // for ROS 2 Jazzy or newer #else diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/test/test.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/test/test.cpp index 295737ed723a5..5c2cbf3957de0 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/test/test.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/test/test.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp index 04823a71febe9..a4ab83cde6dfa 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp @@ -24,6 +24,10 @@ #include #include +#include +#include +#include + namespace landmark_manager { diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp index 947ba02402697..2faf2d19168a5 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -31,9 +31,11 @@ #include #include #include +#include #include #include #include +#include namespace autoware::lidar_marker_localizer { diff --git a/localization/autoware_localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp index 4a7d71909fb7a..847f89e0da9b3 100644 --- a/localization/autoware_localization_util/src/covariance_ellipse.cpp +++ b/localization/autoware_localization_util/src/covariance_ellipse.cpp @@ -15,6 +15,8 @@ #include "autoware/localization_util/covariance_ellipse.hpp" #include + +#include #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp index 064b33ebe3ad9..e9f0318d1e06d 100644 --- a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp +++ b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp @@ -17,7 +17,10 @@ #include #include #include +#include #include +#include +#include namespace autoware::localization_util { diff --git a/localization/autoware_localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp index 6b019f7750471..17187a8d732f0 100644 --- a/localization/autoware_localization_util/src/util_func.cpp +++ b/localization/autoware_localization_util/src/util_func.cpp @@ -16,6 +16,9 @@ #include "autoware/localization_util/matrix_type.hpp" +#include +#include + namespace autoware::localization_util { // ref by http://takacity.blog.fc2.com/blog-entry-69.html diff --git a/localization/autoware_localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp index 6cbe3c2a62571..2d71a385246b7 100644 --- a/localization/autoware_localization_util/test/test_tpe.cpp +++ b/localization/autoware_localization_util/test/test_tpe.cpp @@ -16,6 +16,12 @@ #include +#include +#include +#include +#include +#include + using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator; TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h index 00c567775a6bf..b7c9877739123 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h @@ -67,6 +67,7 @@ #include +#include #include #include diff --git a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index 31e0ae2eb1a59..25b51b55aebd7 100644 --- a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp @@ -14,6 +14,9 @@ #include "autoware/ndt_scan_matcher/map_update_module.hpp" +#include +#include + namespace autoware::ndt_scan_matcher { diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp index 3c6e8a3d5d2f0..d8a576a86879b 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp @@ -14,8 +14,12 @@ #include "autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp" +#include #include #include +#include +#include +#include namespace pclomp { diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index a316c8fa346b9..a1871023d525b 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -25,6 +25,11 @@ #include +#include +#include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/localization/autoware_ndt_scan_matcher/src/particle.cpp b/localization/autoware_ndt_scan_matcher/src/particle.cpp index a5db875fc3ff7..7211d612b7a21 100644 --- a/localization/autoware_ndt_scan_matcher/src/particle.cpp +++ b/localization/autoware_ndt_scan_matcher/src/particle.cpp @@ -15,6 +15,8 @@ #include "autoware/ndt_scan_matcher/particle.hpp" #include "autoware/localization_util/util_func.hpp" + +#include namespace autoware::ndt_scan_matcher { diff --git a/localization/autoware_ndt_scan_matcher/test/test_fixture.hpp b/localization/autoware_ndt_scan_matcher/test/test_fixture.hpp index b0b7dfe50b4ed..f82bb5fd03fea 100644 --- a/localization/autoware_ndt_scan_matcher/test/test_fixture.hpp +++ b/localization/autoware_ndt_scan_matcher/test/test_fixture.hpp @@ -28,6 +28,7 @@ #include #include +#include #include #include #include diff --git a/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp b/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp index 53d6ecb244f3e..1fa24c7b4f99c 100644 --- a/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp +++ b/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace autoware::pose_covariance_modifier { using PoseSource = PoseCovarianceModifierNode::PoseSource; diff --git a/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.cpp index 41ad45430d238..b2e06118b4569 100644 --- a/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.cpp @@ -23,6 +23,8 @@ #include #include +#include +#include #include #include diff --git a/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp index cc5414ebd0a68..023b49c854621 100644 --- a/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp @@ -16,6 +16,9 @@ #include +#include +#include +#include #include namespace autoware::pose_estimator_arbiter::switch_rule diff --git a/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.cpp index 58cf9ab09e9a7..47eb67b1914ce 100644 --- a/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.cpp @@ -16,6 +16,10 @@ #include +#include +#include +#include +#include #include namespace autoware::pose_estimator_arbiter::switch_rule diff --git a/localization/autoware_pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp index 0d1f245123806..27ca4715fd0f1 100644 --- a/localization/autoware_pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp @@ -17,6 +17,7 @@ #include #include +#include #include class PcdMapBasedRuleMockNode : public ::testing::Test diff --git a/localization/autoware_pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp index 10833b5498b89..6925a424cac6c 100644 --- a/localization/autoware_pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp @@ -26,6 +26,8 @@ #include #include +#include +#include #include class RuleHelperMockNode : public ::testing::Test diff --git a/localization/autoware_pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp index 9d537f2048176..e57956b6afae3 100644 --- a/localization/autoware_pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp @@ -22,6 +22,7 @@ #include #include +#include #include class VectorMapBasedRuleMockNode : public ::testing::Test diff --git a/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp index 9108d44e82fb5..c62c533f23093 100644 --- a/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp +++ b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp @@ -22,6 +22,14 @@ #include +#include +#include +#include +#include +#include +#include +#include + namespace autoware::pose_estimator_arbiter { // Parses ros param to get the estimator set that is running diff --git a/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.cpp b/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.cpp index 9767e9ef4fba4..22896d6b8f93b 100644 --- a/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include diff --git a/localization/autoware_pose_initializer/src/localization_module.cpp b/localization/autoware_pose_initializer/src/localization_module.cpp index c6b57dd83bc97..e3fdeec6934fe 100644 --- a/localization/autoware_pose_initializer/src/localization_module.cpp +++ b/localization/autoware_pose_initializer/src/localization_module.cpp @@ -19,6 +19,7 @@ #include #include +#include namespace autoware::pose_initializer { diff --git a/localization/autoware_pose_initializer/test/test_copy_vector_to_array.cpp b/localization/autoware_pose_initializer/test/test_copy_vector_to_array.cpp index 6205eb3b53864..99a07a56c22f7 100644 --- a/localization/autoware_pose_initializer/test/test_copy_vector_to_array.cpp +++ b/localization/autoware_pose_initializer/test/test_copy_vector_to_array.cpp @@ -16,6 +16,8 @@ #include +#include + TEST(CopyVectorToArray, CopyAllElements) { const std::vector vector{0, 1, 2, 3, 4}; diff --git a/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp b/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp index 36aa4f6d7d73e..d191ec2a3bb22 100644 --- a/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp @@ -22,8 +22,10 @@ #include #include +#include #include #include +#include namespace autoware::pose_instability_detector { diff --git a/localization/autoware_pose_instability_detector/test/test.cpp b/localization/autoware_pose_instability_detector/test/test.cpp index bd663a2406903..14ca03e4c6f40 100644 --- a/localization/autoware_pose_instability_detector/test/test.cpp +++ b/localization/autoware_pose_instability_detector/test/test.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include diff --git a/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp b/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp index 222389c32ef31..118bc3cb634c3 100644 --- a/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp +++ b/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp @@ -14,6 +14,8 @@ #include "yabloc_common/camera_info_subscriber.hpp" +#include + namespace yabloc::common { CameraInfoSubscriber::CameraInfoSubscriber(rclcpp::Node * node) diff --git a/localization/yabloc/yabloc_common/src/cv_decompress.cpp b/localization/yabloc/yabloc_common/src/cv_decompress.cpp index 5808d5efc61af..0f4bf2b7bc498 100644 --- a/localization/yabloc/yabloc_common/src/cv_decompress.cpp +++ b/localization/yabloc/yabloc_common/src/cv_decompress.cpp @@ -17,6 +17,8 @@ #include #include +#include + #if __has_include() #include // for ROS 2 Jazzy or newer #else 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 d96285c0d5c00..83edeb4bd9b96 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 @@ -28,7 +28,12 @@ #include #include +#include #include +#include +#include +#include +#include namespace yabloc::ground_server { diff --git a/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp b/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp index 11279f6268092..5a8dadec898d6 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp @@ -16,6 +16,8 @@ #include +#include + namespace yabloc::ground_server { // cppcheck-suppress unusedFunction 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 6c3d43f33440f..cbb6f48450b91 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 @@ -22,6 +22,10 @@ #include +#include +#include +#include + namespace yabloc::ll2_decomposer { Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to_image", options) diff --git a/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp b/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp index 3aeaefad12629..afe9d9d70349b 100644 --- a/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp +++ b/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp @@ -14,6 +14,9 @@ #include "yabloc_common/static_tf_subscriber.hpp" +#include +#include + namespace yabloc::common { StaticTfSubscriber::StaticTfSubscriber(rclcpp::Clock::SharedPtr clock) 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 9539e44f61276..cedfb570fe74c 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 @@ -21,6 +21,11 @@ #include #include +#include +#include +#include +#include + namespace yabloc::graph_segment { GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp index c7bf41bff459b..5483f4a1e6ae8 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp @@ -19,6 +19,9 @@ #include #include +#include +#include +#include namespace yabloc::graph_segment { 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 d31b19406e320..948f5764e2fe3 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 @@ -26,6 +26,9 @@ #include +#include +#include + namespace yabloc::lanelet2_overlay { Lanelet2Overlay::Lanelet2Overlay(const rclcpp::NodeOptions & options) 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 c5cec31e24844..8b4dd71561276 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 @@ -21,6 +21,8 @@ #include +#include + namespace yabloc::line_segment_detector { LineSegmentDetector::LineSegmentDetector(const rclcpp::NodeOptions & options) 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 b844fb69285b5..ab2c241001df7 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 @@ -21,6 +21,11 @@ #include +#include +#include +#include +#include + namespace yabloc::segment_filter { SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) 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 4c33377c8ed0f..98484d5875e14 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -24,6 +24,8 @@ #include #include +#include + #if __has_include() #include // for ROS 2 Jazzy or newer #else diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp index d6cc4553e29bc..b130ef2db1425 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp @@ -29,6 +29,7 @@ #include #include +#include #include #include #include diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp index d6a38e7b97b96..4e8decf1308bf 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp @@ -25,7 +25,10 @@ #include +#include #include +#include +#include namespace yabloc::modularized_particle_filter { diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp index 105f72920a6de..20c825848b728 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp @@ -20,6 +20,8 @@ #include #include +#include + namespace yabloc::modularized_particle_filter { cv::Point2f cv2pt(const Eigen::Vector3f & v) diff --git a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp index 1a0ef05508c76..0219165734967 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp @@ -27,6 +27,7 @@ #include #include #include +#include namespace yabloc::modularized_particle_filter { diff --git a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp index 48f2041595a77..fa08deb0da9de 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp @@ -20,6 +20,8 @@ #include #include +#include + namespace yabloc::modularized_particle_filter { class ParticleVisualize : public rclcpp::Node diff --git a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp index 17b22757b4bc5..e9b85f380cf8c 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp @@ -16,6 +16,8 @@ #include +#include + namespace yabloc::modularized_particle_filter { ParticleVisualizer::ParticleVisualizer(rclcpp::Node & node) diff --git a/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp b/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp index 1c8e2f5976a1d..3c776743824ca 100644 --- a/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp @@ -14,6 +14,10 @@ #include "yabloc_particle_filter/correction/abstract_corrector.hpp" +#include +#include +#include + namespace yabloc::modularized_particle_filter { AbstractCorrector::AbstractCorrector( diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp index c6359a8c8f5fc..4006e8f6fd2c2 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp @@ -14,6 +14,9 @@ #include "yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp" +#include +#include + namespace yabloc { cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity) diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index 4f03e3d26ecb6..7450635b70303 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -22,6 +22,8 @@ #include +#include + namespace yabloc { float Area::unit_length = -1; diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp index 701a2b8763fa3..e92de610e4dec 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp @@ -26,7 +26,9 @@ #include +#include #include +#include namespace yabloc::modularized_particle_filter { diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp index c8c3971ba85b2..f70b4721798be 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp @@ -21,6 +21,7 @@ #include #include #include +#include namespace yabloc::modularized_particle_filter { 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 23f711d1ba17b..0d446e976c089 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 @@ -20,6 +20,10 @@ #include #include +#include +#include +#include + #if __has_include() #include // for ROS 2 Jazzy or newer #else diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp index d6949c43dbfc5..ca4ca60179bd0 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp @@ -23,6 +23,9 @@ #include #include +#include +#include + namespace yabloc { namespace bg = boost::geometry; diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp index 0d4dd8bb373e5..9b479c18db07e 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp @@ -16,6 +16,9 @@ #include +#include +#include + namespace yabloc::initializer { diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp index 449812eff6e09..59226dc0e9171 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp @@ -18,6 +18,8 @@ #include #include +#include + namespace yabloc::initializer { ProjectorModule::ProjectorModule(rclcpp::Node * node) diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp index 917015269d3e2..472d45749d611 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp @@ -20,6 +20,8 @@ #include #include +#include +#include #include struct SemanticSegmentation::Impl diff --git a/map/autoware_lanelet2_map_visualizer/package.xml b/map/autoware_lanelet2_map_visualizer/package.xml index 0244a5f010aa4..3c3bb24c18399 100644 --- a/map/autoware_lanelet2_map_visualizer/package.xml +++ b/map/autoware_lanelet2_map_visualizer/package.xml @@ -24,7 +24,6 @@ autoware_map_msgs rclcpp rclcpp_components - tier4_map_msgs visualization_msgs ament_lint_auto diff --git a/map/autoware_map_height_fitter/src/map_height_fitter.cpp b/map/autoware_map_height_fitter/src/map_height_fitter.cpp index 732f13e375f05..e86674c86f10d 100644 --- a/map/autoware_map_height_fitter/src/map_height_fitter.cpp +++ b/map/autoware_map_height_fitter/src/map_height_fitter.cpp @@ -29,7 +29,9 @@ #include #include +#include #include +#include namespace autoware::map_height_fitter { diff --git a/map/autoware_map_loader/CHANGELOG.rst b/map/autoware_map_loader/CHANGELOG.rst index d7acfba736fd1..8aafae5e8b44b 100644 --- a/map/autoware_map_loader/CHANGELOG.rst +++ b/map/autoware_map_loader/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package map_loader -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_map_loader +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.38.0 (2024-11-08) ------------------- diff --git a/map/autoware_map_loader/README.md b/map/autoware_map_loader/README.md index 046c9062081ca..ec06ee4d824c7 100644 --- a/map/autoware_map_loader/README.md +++ b/map/autoware_map_loader/README.md @@ -132,7 +132,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co 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. +Please see [autoware_map_msgs/msg/MapProjectorInfo.msg](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. ### How to run @@ -140,7 +140,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Subscribed Topics -- ~input/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Projection type for Autoware +- ~input/map_projector_info (autoware_map_msgs/MapProjectorInfo) : Projection type for Autoware ### Published Topics diff --git a/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp b/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp index 54d8244ff76e0..0922b1cb2bdd6 100644 --- a/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp +++ b/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include @@ -40,7 +40,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, - const tier4_map_msgs::msg::MapProjectorInfo & projector_info); + const autoware_map_msgs::msg::MapProjectorInfo & projector_info); static autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); diff --git a/map/autoware_map_loader/package.xml b/map/autoware_map_loader/package.xml index 15fc9fd13df64..7b793359d725b 100644 --- a/map/autoware_map_loader/package.xml +++ b/map/autoware_map_loader/package.xml @@ -30,7 +30,6 @@ pcl_conversions rclcpp rclcpp_components - tier4_map_msgs visualization_msgs yaml-cpp diff --git a/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index ee1cc4a58a1be..47439508f46ae 100644 --- a/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -49,13 +49,14 @@ #include #include +#include #include #include namespace autoware::map_loader { using autoware_map_msgs::msg::LaneletMapBin; -using tier4_map_msgs::msg::MapProjectorInfo; +using autoware_map_msgs::msg::MapProjectorInfo; Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options) : Node("lanelet2_map_loader", options) @@ -136,10 +137,10 @@ void Lanelet2MapLoaderNode::on_map_projector_info( lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( const std::string & lanelet2_filename, - const tier4_map_msgs::msg::MapProjectorInfo & projector_info) + const autoware_map_msgs::msg::MapProjectorInfo & projector_info) { lanelet::ErrorMessages errors{}; - if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { + if (projector_info.projector_type != autoware_map_msgs::msg::MapProjectorInfo::LOCAL) { std::unique_ptr projector = autoware::geography_utils::get_lanelet2_projector(projector_info); lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); diff --git a/map/autoware_map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/autoware_map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 160c6f876138d..30a0dd06364d3 100644 --- a/map/autoware_map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/autoware_map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -14,7 +14,10 @@ #include "differential_map_loader_module.hpp" +#include +#include #include +#include namespace autoware::map_loader { diff --git a/map/autoware_map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/autoware_map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index afa3da7b82b2d..1c96f82b22853 100644 --- a/map/autoware_map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/autoware_map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -14,6 +14,8 @@ #include "partial_map_loader_module.hpp" +#include +#include #include namespace autoware::map_loader diff --git a/map/autoware_map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/autoware_map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 2500d96650ae3..a22654faa712a 100644 --- a/map/autoware_map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/autoware_map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -20,7 +20,9 @@ #include #include +#include #include +#include #include #include diff --git a/map/autoware_map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp b/map/autoware_map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp index 082d1f545dc14..18b8ace1fec68 100644 --- a/map/autoware_map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp +++ b/map/autoware_map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp @@ -14,6 +14,8 @@ #include "selected_map_loader_module.hpp" +#include +#include #include namespace autoware::map_loader diff --git a/map/autoware_map_loader/src/pointcloud_map_loader/utils.cpp b/map/autoware_map_loader/src/pointcloud_map_loader/utils.cpp index 172d462947548..cf52986cf58d8 100644 --- a/map/autoware_map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/autoware_map_loader/src/pointcloud_map_loader/utils.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include diff --git a/map/autoware_map_loader/test/test_differential_map_loader_module.cpp b/map/autoware_map_loader/test/test_differential_map_loader_module.cpp index cd9b670b32fd3..56be0556bdf04 100644 --- a/map/autoware_map_loader/test/test_differential_map_loader_module.cpp +++ b/map/autoware_map_loader/test/test_differential_map_loader_module.cpp @@ -21,7 +21,9 @@ #include #include +#include #include +#include using autoware::map_loader::DifferentialMapLoaderModule; using autoware_map_msgs::srv::GetDifferentialPointCloudMap; diff --git a/map/autoware_map_loader/test/test_load_pcd_metadata.cpp b/map/autoware_map_loader/test/test_load_pcd_metadata.cpp index dc23c3a9fb2d6..eb246c22afbc3 100644 --- a/map/autoware_map_loader/test/test_load_pcd_metadata.cpp +++ b/map/autoware_map_loader/test/test_load_pcd_metadata.cpp @@ -18,6 +18,8 @@ #include #include +#include +#include using ::testing::ContainerEq; diff --git a/map/autoware_map_loader/test/test_partial_map_loader_module.cpp b/map/autoware_map_loader/test/test_partial_map_loader_module.cpp index fd25b600c81af..85a1f82fb6b2f 100644 --- a/map/autoware_map_loader/test/test_partial_map_loader_module.cpp +++ b/map/autoware_map_loader/test/test_partial_map_loader_module.cpp @@ -20,7 +20,9 @@ #include +#include #include +#include using autoware::map_loader::PartialMapLoaderModule; using autoware_map_msgs::srv::GetPartialPointCloudMap; diff --git a/map/autoware_map_loader/test/test_pointcloud_map_loader_module.cpp b/map/autoware_map_loader/test/test_pointcloud_map_loader_module.cpp index d61839d5abfc8..3359847a1751c 100644 --- a/map/autoware_map_loader/test/test_pointcloud_map_loader_module.cpp +++ b/map/autoware_map_loader/test/test_pointcloud_map_loader_module.cpp @@ -25,6 +25,8 @@ #include #include +#include +#include class TestPointcloudMapLoaderModule : public ::testing::Test { diff --git a/map/autoware_map_loader/test/test_replace_with_absolute_path.cpp b/map/autoware_map_loader/test/test_replace_with_absolute_path.cpp index e8ccfbd5fb9f4..00600d51288f6 100644 --- a/map/autoware_map_loader/test/test_replace_with_absolute_path.cpp +++ b/map/autoware_map_loader/test/test_replace_with_absolute_path.cpp @@ -17,6 +17,11 @@ #include #include +#include +#include +#include +#include + using autoware::map_loader::PCDFileMetadata; using autoware::map_loader::replace_with_absolute_path; using ::testing::ContainerEq; diff --git a/map/autoware_map_projection_loader/README.md b/map/autoware_map_projection_loader/README.md index 2568bb2c17df7..ad35a666eb0b8 100644 --- a/map/autoware_map_projection_loader/README.md +++ b/map/autoware_map_projection_loader/README.md @@ -29,7 +29,7 @@ There are three types of transformations from latitude and longitude to XYZ coor ```yaml # map_projector_info.yaml -projector_type: local +projector_type: Local ``` #### Limitation @@ -86,7 +86,7 @@ map_origin: ## Published Topics -- `~/map_projector_info` (tier4_map_msgs/MapProjectorInfo) : This topic shows the definition of map projector information +- `~/map_projector_info` (autoware_map_msgs/MapProjectorInfo) : This topic shows the definition of map projector information ## Parameters diff --git a/map/autoware_map_projection_loader/include/autoware/map_projection_loader/load_info_from_lanelet2_map.hpp b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/load_info_from_lanelet2_map.hpp index 2f1db14529ad0..da0ebb83748ab 100644 --- a/map/autoware_map_projection_loader/include/autoware/map_projection_loader/load_info_from_lanelet2_map.hpp +++ b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/load_info_from_lanelet2_map.hpp @@ -20,13 +20,13 @@ #include #include -#include "tier4_map_msgs/msg/map_projector_info.hpp" +#include "autoware_map_msgs/msg/map_projector_info.hpp" #include namespace autoware::map_projection_loader { -tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename); +autoware_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename); } // namespace autoware::map_projection_loader #endif // AUTOWARE__MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ diff --git a/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp index 6ef9213cf33f8..1ca876dc035b2 100644 --- a/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp +++ b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp @@ -24,8 +24,8 @@ namespace autoware::map_projection_loader { -tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); -tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( +autoware_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); +autoware_map_msgs::msg::MapProjectorInfo load_map_projector_info( const std::string & yaml_filename, const std::string & lanelet2_map_filename); class MapProjectionLoader : public rclcpp::Node diff --git a/map/autoware_map_projection_loader/package.xml b/map/autoware_map_projection_loader/package.xml index 55785a81ea1a2..ddf95efc7414c 100644 --- a/map/autoware_map_projection_loader/package.xml +++ b/map/autoware_map_projection_loader/package.xml @@ -19,9 +19,9 @@ autoware_component_interface_specs autoware_component_interface_utils autoware_lanelet2_extension + autoware_map_msgs rclcpp rclcpp_components - tier4_map_msgs yaml-cpp ament_cmake_gmock diff --git a/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp index ce4cda5c2c677..750a73c8a8ec6 100644 --- a/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp +++ b/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -14,7 +14,7 @@ #include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" -#include "tier4_map_msgs/msg/map_projector_info.hpp" +#include "autoware_map_msgs/msg/map_projector_info.hpp" #include #include @@ -25,7 +25,7 @@ namespace autoware::map_projection_loader { -tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) +autoware_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) { lanelet::ErrorMessages errors{}; lanelet::projection::MGRSProjector projector{}; @@ -46,18 +46,18 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str } } - tier4_map_msgs::msg::MapProjectorInfo msg; + autoware_map_msgs::msg::MapProjectorInfo msg; if (is_local) { - msg.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL; + msg.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL; } else { - msg.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + msg.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS; msg.mgrs_grid = projector.getProjectedMGRSGrid(); } // We assume that the vertical datum of the map is WGS84 when using lanelet2 map. // However, do note that this is not always true, and may cause problems in the future. // Thus, please consider using the map_projector_info.yaml instead of this deprecated function. - msg.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + msg.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; return msg; } } // namespace autoware::map_projection_loader diff --git a/map/autoware_map_projection_loader/src/map_projection_loader.cpp b/map/autoware_map_projection_loader/src/map_projection_loader.cpp index 588ede616a814..981380d1d493b 100644 --- a/map/autoware_map_projection_loader/src/map_projection_loader.cpp +++ b/map/autoware_map_projection_loader/src/map_projection_loader.cpp @@ -16,48 +16,59 @@ #include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" -#include +#include #include #include #include +#include +#include namespace autoware::map_projection_loader { -tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) +autoware_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) { YAML::Node data = YAML::LoadFile(filename); - tier4_map_msgs::msg::MapProjectorInfo msg; + autoware_map_msgs::msg::MapProjectorInfo msg; msg.projector_type = data["projector_type"].as(); - if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::MGRS) { + if (msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::MGRS) { msg.vertical_datum = data["vertical_datum"].as(); msg.mgrs_grid = data["mgrs_grid"].as(); } else if ( - msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM || - msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR) { + msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM || + msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR) { msg.vertical_datum = data["vertical_datum"].as(); msg.map_origin.latitude = data["map_origin"]["latitude"].as(); msg.map_origin.longitude = data["map_origin"]["longitude"].as(); msg.map_origin.altitude = data["map_origin"]["altitude"].as(); - } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { + } else if (msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::LOCAL) { ; // do nothing + } else if (msg.projector_type == "local") { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("MapProjectionLoader"), + "Load " << filename << "\n" + << "DEPRECATED WARNING: projector type \"local\" is deprecated." + "Please use \"Local\" instead. For more info, visit " + "https://github.com/autowarefoundation/autoware.universe/blob/main/map/" + "map_projection_loader README.md"); + msg.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL; } else { throw std::runtime_error( "Invalid map projector type. Currently supported types: MGRS, LocalCartesianUTM, " - "TransverseMercator, and local"); + "TransverseMercator, and Local"); } return msg; } -tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( +autoware_map_msgs::msg::MapProjectorInfo load_map_projector_info( const std::string & yaml_filename, const std::string & lanelet2_map_filename) { - tier4_map_msgs::msg::MapProjectorInfo msg; + autoware_map_msgs::msg::MapProjectorInfo msg; if (std::filesystem::exists(yaml_filename)) { std::cout << "Load " << yaml_filename << std::endl; @@ -87,7 +98,7 @@ MapProjectionLoader::MapProjectionLoader(const rclcpp::NodeOptions & options) const std::string lanelet2_map_filename = this->declare_parameter("lanelet2_map_path"); - const tier4_map_msgs::msg::MapProjectorInfo msg = + const autoware_map_msgs::msg::MapProjectorInfo msg = load_map_projector_info(yaml_filename, lanelet2_map_filename); // Publish the message diff --git a/map/autoware_map_projection_loader/test/data/map_projector_info_local.yaml b/map/autoware_map_projection_loader/test/data/map_projector_info_local.yaml index 2e5d27c3e8143..b1f30b12d8bde 100644 --- a/map/autoware_map_projection_loader/test/data/map_projector_info_local.yaml +++ b/map/autoware_map_projection_loader/test/data/map_projector_info_local.yaml @@ -1 +1 @@ -projector_type: local +projector_type: Local diff --git a/map/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp index 26658e88682a2..bf2207a777642 100644 --- a/map/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp +++ b/map/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -20,6 +20,8 @@ #include #include +#include +#include void save_dummy_mgrs_lanelet2_map(const std::string & mgrs_coord, const std::string & output_path) { @@ -111,7 +113,7 @@ TEST(TestLoadFromLanelet2Map, LoadLocalGrid) autoware::map_projection_loader::load_info_from_lanelet2_map(output_path); // Check the result - EXPECT_EQ(projector_info.projector_type, "local"); + EXPECT_EQ(projector_info.projector_type, "Local"); } TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid) diff --git a/map/autoware_map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/autoware_map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py index 495f0092bb01f..30cede4b1ec25 100644 --- a/map/autoware_map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py +++ b/map/autoware_map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -18,6 +18,7 @@ import unittest from ament_index_python import get_package_share_directory +from autoware_map_msgs.msg import MapProjectorInfo import launch from launch import LaunchDescription from launch.logging import get_logger @@ -28,7 +29,6 @@ from rclpy.qos import QoSDurabilityPolicy from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile -from tier4_map_msgs.msg import MapProjectorInfo import yaml logger = get_logger(__name__) diff --git a/map/autoware_map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/autoware_map_projection_loader/test/test_node_load_local_from_yaml.test.py index 8517f09e2006b..9c364f094ee24 100644 --- a/map/autoware_map_projection_loader/test/test_node_load_local_from_yaml.test.py +++ b/map/autoware_map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -18,6 +18,7 @@ import unittest from ament_index_python import get_package_share_directory +from autoware_map_msgs.msg import MapProjectorInfo import launch from launch import LaunchDescription from launch.logging import get_logger @@ -28,7 +29,6 @@ from rclpy.qos import QoSDurabilityPolicy from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile -from tier4_map_msgs.msg import MapProjectorInfo import yaml logger = get_logger(__name__) diff --git a/map/autoware_map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/autoware_map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index ad19e61f9f111..6586a9aee98d3 100644 --- a/map/autoware_map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/autoware_map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -18,6 +18,7 @@ import unittest from ament_index_python import get_package_share_directory +from autoware_map_msgs.msg import MapProjectorInfo import launch from launch import LaunchDescription from launch.logging import get_logger @@ -28,7 +29,6 @@ from rclpy.qos import QoSDurabilityPolicy from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile -from tier4_map_msgs.msg import MapProjectorInfo import yaml logger = get_logger(__name__) diff --git a/map/autoware_map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/autoware_map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py index ed2eb45535089..3035ce75e692d 100644 --- a/map/autoware_map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py +++ b/map/autoware_map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py @@ -18,6 +18,7 @@ import unittest from ament_index_python import get_package_share_directory +from autoware_map_msgs.msg import MapProjectorInfo import launch from launch import LaunchDescription from launch.logging import get_logger @@ -28,7 +29,6 @@ from rclpy.qos import QoSDurabilityPolicy from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile -from tier4_map_msgs.msg import MapProjectorInfo import yaml logger = get_logger(__name__) diff --git a/map/autoware_map_tf_generator/src/pcd_map_tf_generator_node.cpp b/map/autoware_map_tf_generator/src/pcd_map_tf_generator_node.cpp index e6edbe99dd210..f1e6b75d2d309 100644 --- a/map/autoware_map_tf_generator/src/pcd_map_tf_generator_node.cpp +++ b/map/autoware_map_tf_generator/src/pcd_map_tf_generator_node.cpp @@ -27,6 +27,7 @@ #include #include #include +#include namespace autoware::map_tf_generator { diff --git a/map/autoware_map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/autoware_map_tf_generator/src/vector_map_tf_generator_node.cpp index e4c397bd04cf1..e31416fb1a2d3 100644 --- a/map/autoware_map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/autoware_map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -24,6 +24,7 @@ #include #include +#include namespace autoware::map_tf_generator { diff --git a/map/autoware_map_tf_generator/test/test_uniform_random.cpp b/map/autoware_map_tf_generator/test/test_uniform_random.cpp index 481f5c6d86859..29557d9327e5a 100644 --- a/map/autoware_map_tf_generator/test/test_uniform_random.cpp +++ b/map/autoware_map_tf_generator/test/test_uniform_random.cpp @@ -16,6 +16,8 @@ #include +#include + using testing::AllOf; using testing::Each; using testing::Ge; diff --git a/mkdocs.yaml b/mkdocs.yaml index 2ca8b3c86ad43..38786709319be 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + site_name: Autoware Universe Documentation site_url: https://autowarefoundation.github.io/autoware.universe repo_url: https://github.com/autowarefoundation/autoware.universe diff --git a/perception/autoware_bytetrack/launch/bytetrack.launch.xml b/perception/autoware_bytetrack/launch/bytetrack.launch.xml index ace84d4799a8a..c009ed93754fa 100644 --- a/perception/autoware_bytetrack/launch/bytetrack.launch.xml +++ b/perception/autoware_bytetrack/launch/bytetrack.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/perception/autoware_bytetrack/lib/include/byte_tracker.h b/perception/autoware_bytetrack/lib/include/byte_tracker.h index 60328c5acfd62..736ed6d311043 100644 --- a/perception/autoware_bytetrack/lib/include/byte_tracker.h +++ b/perception/autoware_bytetrack/lib/include/byte_tracker.h @@ -40,6 +40,7 @@ #include "strack.h" +#include #include struct ByteTrackObject @@ -83,8 +84,8 @@ class ByteTracker double lapjv( const std::vector> & cost, std::vector & rowsol, - std::vector & colsol, bool extend_cost = false, float cost_limit = LONG_MAX, - bool return_cost = true); + std::vector & colsol, bool extend_cost = false, + float cost_limit = std::numeric_limits::max(), bool return_cost = true); private: float track_thresh; diff --git a/perception/autoware_bytetrack/lib/include/lapjv.h b/perception/autoware_bytetrack/lib/include/lapjv.h index d197b747e6f7d..c42fcbac8eb60 100644 --- a/perception/autoware_bytetrack/lib/include/lapjv.h +++ b/perception/autoware_bytetrack/lib/include/lapjv.h @@ -64,40 +64,10 @@ b = _temp_index; \ } -#if 0 -#include -#define ASSERT(cond) assert(cond) -#define PRINTF(fmt, ...) printf(fmt, ##__VA_ARGS__) -#define PRINT_COST_ARRAY(a, n) \ - while (1) { \ - printf(#a " = ["); \ - if ((n) > 0) { \ - printf("%f", (a)[0]); \ - for (uint_t j = 1; j < n; j++) { \ - printf(", %f", (a)[j]); \ - } \ - } \ - printf("]\n"); \ - break; \ - } -#define PRINT_INDEX_ARRAY(a, n) \ - while (1) { \ - printf(#a " = ["); \ - if ((n) > 0) { \ - printf("%d", (a)[0]); \ - for (uint_t j = 1; j < n; j++) { \ - printf(", %d", (a)[j]); \ - } \ - } \ - printf("]\n"); \ - break; \ - } -#else #define ASSERT(cond) #define PRINTF(fmt, ...) #define PRINT_COST_ARRAY(a, n) #define PRINT_INDEX_ARRAY(a, n) -#endif typedef signed int int_t; typedef unsigned int uint_t; diff --git a/perception/autoware_bytetrack/lib/src/byte_tracker.cpp b/perception/autoware_bytetrack/lib/src/byte_tracker.cpp index a3477e78ad6dd..1d1288cbb2a48 100644 --- a/perception/autoware_bytetrack/lib/src/byte_tracker.cpp +++ b/perception/autoware_bytetrack/lib/src/byte_tracker.cpp @@ -1,248 +1,250 @@ -/* - * MIT License - * - * Copyright (c) 2021 Yifu Zhang - * - * 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. - */ - -// 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 "byte_tracker.h" - -#include -#include - -ByteTracker::ByteTracker( - int track_buffer, float track_thresh, float high_thresh, float match_thresh) -: track_thresh(track_thresh), high_thresh(high_thresh), match_thresh(match_thresh) - -{ - frame_id = 0; - max_time_lost = track_buffer; - std::cout << "Init ByteTrack!" << std::endl; -} - -ByteTracker::~ByteTracker() -{ -} - -std::vector ByteTracker::update(const std::vector & objects) -{ - ////////////////// Step 1: Get detections ////////////////// - this->frame_id++; - std::vector activated_stracks; - std::vector refind_stracks; - std::vector removed_stracks; - std::vector lost_stracks; - std::vector detections; - std::vector detections_low; - - std::vector detections_cp; - std::vector tracked_stracks_swap; - std::vector resa, resb; - std::vector output_stracks; - - std::vector unconfirmed; - std::vector tracked_stracks; - std::vector strack_pool; - std::vector r_tracked_stracks; - - if (objects.size() > 0) { - for (size_t i = 0; i < objects.size(); i++) { - std::vector tlbr_; - tlbr_.resize(4); - tlbr_[0] = objects[i].rect.x; - tlbr_[1] = objects[i].rect.y; - tlbr_[2] = objects[i].rect.x + objects[i].rect.width; - tlbr_[3] = objects[i].rect.y + objects[i].rect.height; - - float score = objects[i].prob; - - STrack strack(STrack::tlbr_to_tlwh(tlbr_), score, objects[i].label); - if (score >= track_thresh) { - detections.push_back(strack); - } else { - detections_low.push_back(strack); - } - } - } - - // Add newly detected tracklets to tracked_stracks - for (size_t i = 0; i < this->tracked_stracks.size(); i++) { - if (!this->tracked_stracks[i].is_activated) - unconfirmed.push_back(&this->tracked_stracks[i]); - else - tracked_stracks.push_back(&this->tracked_stracks[i]); - } - - ////////////////// Step 2: First association, with IoU ////////////////// - strack_pool = joint_stracks(tracked_stracks, this->lost_stracks); - // do prediction for each stracks - for (size_t i = 0; i < strack_pool.size(); i++) { - strack_pool[i]->predict(this->frame_id); - } - - std::vector > dists; - int dist_size = 0, dist_size_size = 0; - dists = iou_distance(strack_pool, detections, dist_size, dist_size_size); - - std::vector > matches; - std::vector u_track, u_detection; - linear_assignment(dists, dist_size, dist_size_size, match_thresh, matches, u_track, u_detection); - - for (size_t i = 0; i < matches.size(); i++) { - STrack * track = strack_pool[matches[i][0]]; - STrack * det = &detections[matches[i][1]]; - if (track->state == TrackState::Tracked) { - track->update(*det, this->frame_id); - activated_stracks.push_back(*track); - } else { - track->re_activate(*det, this->frame_id, false); - refind_stracks.push_back(*track); - } - } - - ////////////////// Step 3: Second association, using low score dets ////////////////// - for (size_t i = 0; i < u_detection.size(); i++) { - detections_cp.push_back(detections[u_detection[i]]); - } - detections.clear(); - detections.assign(detections_low.begin(), detections_low.end()); - - for (size_t i = 0; i < u_track.size(); i++) { - if (strack_pool[u_track[i]]->state == TrackState::Tracked) { - r_tracked_stracks.push_back(strack_pool[u_track[i]]); - } - } - - dists.clear(); - dists = iou_distance(r_tracked_stracks, detections, dist_size, dist_size_size); - - matches.clear(); - u_track.clear(); - u_detection.clear(); - linear_assignment(dists, dist_size, dist_size_size, 0.5, matches, u_track, u_detection); - - for (size_t i = 0; i < matches.size(); i++) { - STrack * track = r_tracked_stracks[matches[i][0]]; - STrack * det = &detections[matches[i][1]]; - if (track->state == TrackState::Tracked) { - track->update(*det, this->frame_id); - activated_stracks.push_back(*track); - } else { - track->re_activate(*det, this->frame_id, false); - refind_stracks.push_back(*track); - } - } - - for (size_t i = 0; i < u_track.size(); i++) { - STrack * track = r_tracked_stracks[u_track[i]]; - if (track->state != TrackState::Lost) { - track->mark_lost(); - lost_stracks.push_back(*track); - } - } - - // Deal with unconfirmed tracks, usually tracks with only one beginning frame - detections.clear(); - detections.assign(detections_cp.begin(), detections_cp.end()); - - dists.clear(); - dists = iou_distance(unconfirmed, detections, dist_size, dist_size_size); - - matches.clear(); - std::vector u_unconfirmed; - u_detection.clear(); - linear_assignment(dists, dist_size, dist_size_size, 0.7, matches, u_unconfirmed, u_detection); - - for (size_t i = 0; i < matches.size(); i++) { - unconfirmed[matches[i][0]]->update(detections[matches[i][1]], this->frame_id); - activated_stracks.push_back(*unconfirmed[matches[i][0]]); - } - - for (size_t i = 0; i < u_unconfirmed.size(); i++) { - STrack * track = unconfirmed[u_unconfirmed[i]]; - track->mark_removed(); - removed_stracks.push_back(*track); - } - - ////////////////// Step 4: Init new stracks ////////////////// - for (size_t i = 0; i < u_detection.size(); i++) { - STrack * track = &detections[u_detection[i]]; - if (track->score < this->high_thresh) continue; - track->activate(this->frame_id); - activated_stracks.push_back(*track); - } - - ////////////////// Step 5: Update state ////////////////// - for (size_t i = 0; i < this->lost_stracks.size(); i++) { - if (this->frame_id - this->lost_stracks[i].end_frame() > this->max_time_lost) { - this->lost_stracks[i].mark_removed(); - removed_stracks.push_back(this->lost_stracks[i]); - } - } - - for (size_t i = 0; i < this->tracked_stracks.size(); i++) { - if (this->tracked_stracks[i].state == TrackState::Tracked) { - tracked_stracks_swap.push_back(this->tracked_stracks[i]); - } - } - this->tracked_stracks.clear(); - this->tracked_stracks.assign(tracked_stracks_swap.begin(), tracked_stracks_swap.end()); - - this->tracked_stracks = joint_stracks(this->tracked_stracks, activated_stracks); - this->tracked_stracks = joint_stracks(this->tracked_stracks, refind_stracks); - - this->lost_stracks = sub_stracks(this->lost_stracks, this->tracked_stracks); - for (size_t i = 0; i < lost_stracks.size(); i++) { - this->lost_stracks.push_back(lost_stracks[i]); - } - - this->lost_stracks = sub_stracks(this->lost_stracks, this->removed_stracks); - for (size_t i = 0; i < removed_stracks.size(); i++) { - this->removed_stracks.push_back(removed_stracks[i]); - } - - remove_duplicate_stracks(resa, resb, this->tracked_stracks, this->lost_stracks); - - this->tracked_stracks.clear(); - this->tracked_stracks.assign(resa.begin(), resa.end()); - this->lost_stracks.clear(); - this->lost_stracks.assign(resb.begin(), resb.end()); - - for (size_t i = 0; i < this->tracked_stracks.size(); i++) { - if (this->tracked_stracks[i].is_activated) { - output_stracks.push_back(this->tracked_stracks[i]); - } - } - return output_stracks; -} +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * 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. + */ + +// 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 "byte_tracker.h" + +#include +#include +#include +#include + +ByteTracker::ByteTracker( + int track_buffer, float track_thresh, float high_thresh, float match_thresh) +: track_thresh(track_thresh), high_thresh(high_thresh), match_thresh(match_thresh) + +{ + frame_id = 0; + max_time_lost = track_buffer; + std::cout << "Init ByteTrack!" << std::endl; +} + +ByteTracker::~ByteTracker() +{ +} + +std::vector ByteTracker::update(const std::vector & objects) +{ + ////////////////// Step 1: Get detections ////////////////// + this->frame_id++; + std::vector activated_stracks; + std::vector refind_stracks; + std::vector removed_stracks; + std::vector lost_stracks; + std::vector detections; + std::vector detections_low; + + std::vector detections_cp; + std::vector tracked_stracks_swap; + std::vector resa, resb; + std::vector output_stracks; + + std::vector unconfirmed; + std::vector tracked_stracks; + std::vector strack_pool; + std::vector r_tracked_stracks; + + if (objects.size() > 0) { + for (size_t i = 0; i < objects.size(); i++) { + std::vector tlbr_; + tlbr_.resize(4); + tlbr_[0] = objects[i].rect.x; + tlbr_[1] = objects[i].rect.y; + tlbr_[2] = objects[i].rect.x + objects[i].rect.width; + tlbr_[3] = objects[i].rect.y + objects[i].rect.height; + + float score = objects[i].prob; + + STrack strack(STrack::tlbr_to_tlwh(tlbr_), score, objects[i].label); + if (score >= track_thresh) { + detections.push_back(strack); + } else { + detections_low.push_back(strack); + } + } + } + + // Add newly detected tracklets to tracked_stracks + for (size_t i = 0; i < this->tracked_stracks.size(); i++) { + if (!this->tracked_stracks[i].is_activated) + unconfirmed.push_back(&this->tracked_stracks[i]); + else + tracked_stracks.push_back(&this->tracked_stracks[i]); + } + + ////////////////// Step 2: First association, with IoU ////////////////// + strack_pool = joint_stracks(tracked_stracks, this->lost_stracks); + // do prediction for each stracks + for (size_t i = 0; i < strack_pool.size(); i++) { + strack_pool[i]->predict(this->frame_id); + } + + std::vector > dists; + int dist_size = 0, dist_size_size = 0; + dists = iou_distance(strack_pool, detections, dist_size, dist_size_size); + + std::vector > matches; + std::vector u_track, u_detection; + linear_assignment(dists, dist_size, dist_size_size, match_thresh, matches, u_track, u_detection); + + for (size_t i = 0; i < matches.size(); i++) { + STrack * track = strack_pool[matches[i][0]]; + STrack * det = &detections[matches[i][1]]; + if (track->state == TrackState::Tracked) { + track->update(*det, this->frame_id); + activated_stracks.push_back(*track); + } else { + track->re_activate(*det, this->frame_id, false); + refind_stracks.push_back(*track); + } + } + + ////////////////// Step 3: Second association, using low score dets ////////////////// + for (size_t i = 0; i < u_detection.size(); i++) { + detections_cp.push_back(detections[u_detection[i]]); + } + detections.clear(); + detections.assign(detections_low.begin(), detections_low.end()); + + for (size_t i = 0; i < u_track.size(); i++) { + if (strack_pool[u_track[i]]->state == TrackState::Tracked) { + r_tracked_stracks.push_back(strack_pool[u_track[i]]); + } + } + + dists.clear(); + dists = iou_distance(r_tracked_stracks, detections, dist_size, dist_size_size); + + matches.clear(); + u_track.clear(); + u_detection.clear(); + linear_assignment(dists, dist_size, dist_size_size, 0.5, matches, u_track, u_detection); + + for (size_t i = 0; i < matches.size(); i++) { + STrack * track = r_tracked_stracks[matches[i][0]]; + STrack * det = &detections[matches[i][1]]; + if (track->state == TrackState::Tracked) { + track->update(*det, this->frame_id); + activated_stracks.push_back(*track); + } else { + track->re_activate(*det, this->frame_id, false); + refind_stracks.push_back(*track); + } + } + + for (size_t i = 0; i < u_track.size(); i++) { + STrack * track = r_tracked_stracks[u_track[i]]; + if (track->state != TrackState::Lost) { + track->mark_lost(); + lost_stracks.push_back(*track); + } + } + + // Deal with unconfirmed tracks, usually tracks with only one beginning frame + detections.clear(); + detections.assign(detections_cp.begin(), detections_cp.end()); + + dists.clear(); + dists = iou_distance(unconfirmed, detections, dist_size, dist_size_size); + + matches.clear(); + std::vector u_unconfirmed; + u_detection.clear(); + linear_assignment(dists, dist_size, dist_size_size, 0.7, matches, u_unconfirmed, u_detection); + + for (size_t i = 0; i < matches.size(); i++) { + unconfirmed[matches[i][0]]->update(detections[matches[i][1]], this->frame_id); + activated_stracks.push_back(*unconfirmed[matches[i][0]]); + } + + for (size_t i = 0; i < u_unconfirmed.size(); i++) { + STrack * track = unconfirmed[u_unconfirmed[i]]; + track->mark_removed(); + removed_stracks.push_back(*track); + } + + ////////////////// Step 4: Init new stracks ////////////////// + for (size_t i = 0; i < u_detection.size(); i++) { + STrack * track = &detections[u_detection[i]]; + if (track->score < this->high_thresh) continue; + track->activate(this->frame_id); + activated_stracks.push_back(*track); + } + + ////////////////// Step 5: Update state ////////////////// + for (size_t i = 0; i < this->lost_stracks.size(); i++) { + if (this->frame_id - this->lost_stracks[i].end_frame() > this->max_time_lost) { + this->lost_stracks[i].mark_removed(); + removed_stracks.push_back(this->lost_stracks[i]); + } + } + + for (size_t i = 0; i < this->tracked_stracks.size(); i++) { + if (this->tracked_stracks[i].state == TrackState::Tracked) { + tracked_stracks_swap.push_back(this->tracked_stracks[i]); + } + } + this->tracked_stracks.clear(); + this->tracked_stracks.assign(tracked_stracks_swap.begin(), tracked_stracks_swap.end()); + + this->tracked_stracks = joint_stracks(this->tracked_stracks, activated_stracks); + this->tracked_stracks = joint_stracks(this->tracked_stracks, refind_stracks); + + this->lost_stracks = sub_stracks(this->lost_stracks, this->tracked_stracks); + for (size_t i = 0; i < lost_stracks.size(); i++) { + this->lost_stracks.push_back(lost_stracks[i]); + } + + this->lost_stracks = sub_stracks(this->lost_stracks, this->removed_stracks); + for (size_t i = 0; i < removed_stracks.size(); i++) { + this->removed_stracks.push_back(removed_stracks[i]); + } + + remove_duplicate_stracks(resa, resb, this->tracked_stracks, this->lost_stracks); + + this->tracked_stracks.clear(); + this->tracked_stracks.assign(resa.begin(), resa.end()); + this->lost_stracks.clear(); + this->lost_stracks.assign(resb.begin(), resb.end()); + + for (size_t i = 0; i < this->tracked_stracks.size(); i++) { + if (this->tracked_stracks[i].is_activated) { + output_stracks.push_back(this->tracked_stracks[i]); + } + } + return output_stracks; +} diff --git a/perception/autoware_bytetrack/lib/src/strack.cpp b/perception/autoware_bytetrack/lib/src/strack.cpp index 4f1e03a4c1d4a..14e2ada387d90 100644 --- a/perception/autoware_bytetrack/lib/src/strack.cpp +++ b/perception/autoware_bytetrack/lib/src/strack.cpp @@ -44,6 +44,10 @@ #include +#include +#include +#include + // init static variable bool STrack::_parameters_loaded = false; STrack::KfParams STrack::_kf_parameters; diff --git a/perception/autoware_bytetrack/lib/src/utils.cpp b/perception/autoware_bytetrack/lib/src/utils.cpp index e987bd36b8064..3290b46842bf5 100644 --- a/perception/autoware_bytetrack/lib/src/utils.cpp +++ b/perception/autoware_bytetrack/lib/src/utils.cpp @@ -1,398 +1,403 @@ -/* - * MIT License - * - * Copyright (c) 2021 Yifu Zhang - * - * 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. - */ - -// 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 "byte_tracker.h" -#include "lapjv.h" - -#include - -std::vector ByteTracker::joint_stracks( - std::vector & tlista, std::vector & tlistb) -{ - std::map exists; - std::vector res; - for (size_t i = 0; i < tlista.size(); i++) { - exists.insert(std::pair(tlista[i]->track_id, 1)); - res.push_back(tlista[i]); - } - for (size_t i = 0; i < tlistb.size(); i++) { - int tid = tlistb[i].track_id; - if (!exists[tid] || exists.count(tid) == 0) { - exists[tid] = 1; - res.push_back(&tlistb[i]); - } - } - return res; -} - -std::vector ByteTracker::joint_stracks( - std::vector & tlista, std::vector & tlistb) -{ - std::map exists; - std::vector res; - for (size_t i = 0; i < tlista.size(); i++) { - exists.insert(std::pair(tlista[i].track_id, 1)); - res.push_back(tlista[i]); - } - for (size_t i = 0; i < tlistb.size(); i++) { - int tid = tlistb[i].track_id; - if (!exists[tid] || exists.count(tid) == 0) { - exists[tid] = 1; - res.push_back(tlistb[i]); - } - } - return res; -} - -std::vector ByteTracker::sub_stracks( - std::vector & tlista, std::vector & tlistb) -{ - std::map stracks; - for (size_t i = 0; i < tlista.size(); i++) { - stracks.insert(std::pair(tlista[i].track_id, tlista[i])); - } - for (size_t i = 0; i < tlistb.size(); i++) { - int tid = tlistb[i].track_id; - if (stracks.count(tid) != 0) { - stracks.erase(tid); - } - } - - std::vector res; - std::map::iterator it; - for (it = stracks.begin(); it != stracks.end(); ++it) { - res.push_back(it->second); - } - - return res; -} - -void ByteTracker::remove_duplicate_stracks( - std::vector & resa, std::vector & resb, std::vector & stracksa, - std::vector & stracksb) -{ - std::vector> pdist = iou_distance(stracksa, stracksb); - std::vector> pairs; - for (size_t i = 0; i < pdist.size(); i++) { - for (size_t j = 0; j < pdist[i].size(); j++) { - if (pdist[i][j] < 0.15) { - pairs.push_back(std::pair(i, j)); - } - } - } - - std::vector dupa, dupb; - for (size_t i = 0; i < pairs.size(); i++) { - int timep = stracksa[pairs[i].first].frame_id - stracksa[pairs[i].first].start_frame; - int timeq = stracksb[pairs[i].second].frame_id - stracksb[pairs[i].second].start_frame; - if (timep > timeq) - dupb.push_back(pairs[i].second); - else - dupa.push_back(pairs[i].first); - } - - for (size_t i = 0; i < stracksa.size(); i++) { - std::vector::iterator iter = find(dupa.begin(), dupa.end(), i); - if (iter == dupa.end()) { - resa.push_back(stracksa[i]); - } - } - - for (size_t i = 0; i < stracksb.size(); i++) { - std::vector::iterator iter = find(dupb.begin(), dupb.end(), i); - if (iter == dupb.end()) { - resb.push_back(stracksb[i]); - } - } -} - -void ByteTracker::linear_assignment( - std::vector> & cost_matrix, int cost_matrix_size, int cost_matrix_size_size, - float thresh, std::vector> & matches, std::vector & unmatched_a, - std::vector & unmatched_b) -{ - if (cost_matrix.size() == 0) { - for (int i = 0; i < cost_matrix_size; i++) { - unmatched_a.push_back(i); - } - for (int i = 0; i < cost_matrix_size_size; i++) { - unmatched_b.push_back(i); - } - return; - } - - std::vector rowsol; - std::vector colsol; - [[maybe_unused]] float c = lapjv(cost_matrix, rowsol, colsol, true, thresh); - for (size_t i = 0; i < rowsol.size(); i++) { - if (rowsol[i] >= 0) { - std::vector match; - match.push_back(i); - match.push_back(rowsol[i]); - matches.push_back(match); - } else { - unmatched_a.push_back(i); - } - } - - for (size_t i = 0; i < colsol.size(); i++) { - if (colsol[i] < 0) { - unmatched_b.push_back(i); - } - } -} - -std::vector> ByteTracker::ious( - std::vector> & atlbrs, std::vector> & btlbrs) -{ - std::vector> ious; - if (atlbrs.size() * btlbrs.size() == 0) return ious; - - ious.resize(atlbrs.size()); - for (size_t i = 0; i < ious.size(); i++) { - ious[i].resize(btlbrs.size()); - } - - // bbox_ious - for (size_t k = 0; k < btlbrs.size(); k++) { - float box_area = (btlbrs[k][2] - btlbrs[k][0] + 1) * (btlbrs[k][3] - btlbrs[k][1] + 1); - for (size_t n = 0; n < atlbrs.size(); n++) { - float iw = std::min(atlbrs[n][2], btlbrs[k][2]) - std::max(atlbrs[n][0], btlbrs[k][0]) + 1; - if (iw > 0) { - float ih = std::min(atlbrs[n][3], btlbrs[k][3]) - std::max(atlbrs[n][1], btlbrs[k][1]) + 1; - if (ih > 0) { - float ua = (atlbrs[n][2] - atlbrs[n][0] + 1) * (atlbrs[n][3] - atlbrs[n][1] + 1) + - box_area - iw * ih; - ious[n][k] = iw * ih / ua; - } else { - ious[n][k] = 0.0; - } - } else { - ious[n][k] = 0.0; - } - } - } - - return ious; -} - -std::vector> ByteTracker::iou_distance( - std::vector & atracks, std::vector & btracks, int & dist_size, - int & dist_size_size) -{ - std::vector> cost_matrix; - if (atracks.size() * btracks.size() == 0) { - dist_size = atracks.size(); - dist_size_size = btracks.size(); - return cost_matrix; - } - std::vector> atlbrs, btlbrs; - for (size_t i = 0; i < atracks.size(); i++) { - atlbrs.push_back(atracks[i]->tlbr); - } - for (size_t i = 0; i < btracks.size(); i++) { - btlbrs.push_back(btracks[i].tlbr); - } - - dist_size = atracks.size(); - dist_size_size = btracks.size(); - - std::vector> _ious = ious(atlbrs, btlbrs); - - for (size_t i = 0; i < _ious.size(); i++) { - std::vector _iou; - for (size_t j = 0; j < _ious[i].size(); j++) { - _iou.push_back(1 - _ious[i][j]); - } - cost_matrix.push_back(_iou); - } - - return cost_matrix; -} - -std::vector> ByteTracker::iou_distance( - std::vector & atracks, std::vector & btracks) -{ - std::vector> atlbrs, btlbrs; - for (size_t i = 0; i < atracks.size(); i++) { - atlbrs.push_back(atracks[i].tlbr); - } - for (size_t i = 0; i < btracks.size(); i++) { - btlbrs.push_back(btracks[i].tlbr); - } - - std::vector> _ious = ious(atlbrs, btlbrs); - std::vector> cost_matrix; - for (size_t i = 0; i < _ious.size(); i++) { - std::vector _iou; - for (size_t j = 0; j < _ious[i].size(); j++) { - _iou.push_back(1 - _ious[i][j]); - } - cost_matrix.push_back(_iou); - } - - return cost_matrix; -} - -double ByteTracker::lapjv( - const std::vector> & cost, std::vector & rowsol, - std::vector & colsol, bool extend_cost, float cost_limit, bool return_cost) -{ - std::vector> cost_c; - cost_c.assign(cost.begin(), cost.end()); - - int n_rows = cost.size(); - int n_cols = cost[0].size(); - rowsol.resize(n_rows); - colsol.resize(n_cols); - - int n = 0; - if (n_rows == n_cols) { - n = n_rows; - } else { - if (!extend_cost) { - std::cout << "set extend_cost=True" << std::endl; - // system("pause"); - exit(0); - } - } - - if (extend_cost || cost_limit < LONG_MAX) { - std::vector> cost_c_extended; - - n = n_rows + n_cols; - cost_c_extended.resize(n); - for (size_t i = 0; i < cost_c_extended.size(); i++) cost_c_extended[i].resize(n); - - if (cost_limit < LONG_MAX) { - for (size_t i = 0; i < cost_c_extended.size(); i++) { - for (size_t j = 0; j < cost_c_extended[i].size(); j++) { - cost_c_extended[i][j] = cost_limit / 2.0; - } - } - } else { - float cost_max = -1; - for (size_t i = 0; i < cost_c.size(); i++) { - for (size_t j = 0; j < cost_c[i].size(); j++) { - if (cost_c[i][j] > cost_max) cost_max = cost_c[i][j]; - } - } - for (size_t i = 0; i < cost_c_extended.size(); i++) { - for (size_t j = 0; j < cost_c_extended[i].size(); j++) { - cost_c_extended[i][j] = cost_max + 1; - } - } - } - - for (size_t i = n_rows; i < cost_c_extended.size(); i++) { - for (size_t j = n_cols; j < cost_c_extended[i].size(); j++) { - cost_c_extended[i][j] = 0; - } - } - for (int i = 0; i < n_rows; i++) { - for (int j = 0; j < n_cols; j++) { - cost_c_extended[i][j] = cost_c[i][j]; - } - } - - cost_c.clear(); - cost_c.assign(cost_c_extended.begin(), cost_c_extended.end()); - } - - double ** cost_ptr; - cost_ptr = new double *[sizeof(double *) * n]; - for (int i = 0; i < n; i++) cost_ptr[i] = new double[sizeof(double) * n]; - - for (int i = 0; i < n; i++) { - for (int j = 0; j < n; j++) { - cost_ptr[i][j] = cost_c[i][j]; - } - } - - int * x_c = new int[sizeof(int) * n]; - int * y_c = new int[sizeof(int) * n]; - - int ret = lapjv_internal(n, cost_ptr, x_c, y_c); - if (ret != 0) { - std::cout << "Calculate Wrong!" << std::endl; - // system("pause"); - exit(0); - } - - double opt = 0.0; - - if (n != n_rows) { - for (int i = 0; i < n; i++) { - if (x_c[i] >= n_cols) x_c[i] = -1; - if (y_c[i] >= n_rows) y_c[i] = -1; - } - for (int i = 0; i < n_rows; i++) { - rowsol[i] = x_c[i]; - } - for (int i = 0; i < n_cols; i++) { - colsol[i] = y_c[i]; - } - - if (return_cost) { - for (size_t i = 0; i < rowsol.size(); i++) { - if (rowsol[i] != -1) { - opt += cost_ptr[i][rowsol[i]]; - } - } - } - } else if (return_cost) { - for (size_t i = 0; i < rowsol.size(); i++) { - opt += cost_ptr[i][rowsol[i]]; - } - } - - for (int i = 0; i < n; i++) { - delete[] cost_ptr[i]; - } - delete[] cost_ptr; - delete[] x_c; - delete[] y_c; - - return opt; -} - -cv::Scalar ByteTracker::get_color(int idx) -{ - idx += 3; - return cv::Scalar(37 * idx % 255, 17 * idx % 255, 29 * idx % 255); -} +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * 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. + */ + +// 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 "byte_tracker.h" +#include "lapjv.h" + +#include +#include +#include +#include +#include +#include + +std::vector ByteTracker::joint_stracks( + std::vector & tlista, std::vector & tlistb) +{ + std::map exists; + std::vector res; + for (size_t i = 0; i < tlista.size(); i++) { + exists.insert(std::pair(tlista[i]->track_id, 1)); + res.push_back(tlista[i]); + } + for (size_t i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (!exists[tid] || exists.count(tid) == 0) { + exists[tid] = 1; + res.push_back(&tlistb[i]); + } + } + return res; +} + +std::vector ByteTracker::joint_stracks( + std::vector & tlista, std::vector & tlistb) +{ + std::map exists; + std::vector res; + for (size_t i = 0; i < tlista.size(); i++) { + exists.insert(std::pair(tlista[i].track_id, 1)); + res.push_back(tlista[i]); + } + for (size_t i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (!exists[tid] || exists.count(tid) == 0) { + exists[tid] = 1; + res.push_back(tlistb[i]); + } + } + return res; +} + +std::vector ByteTracker::sub_stracks( + std::vector & tlista, std::vector & tlistb) +{ + std::map stracks; + for (size_t i = 0; i < tlista.size(); i++) { + stracks.insert(std::pair(tlista[i].track_id, tlista[i])); + } + for (size_t i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (stracks.count(tid) != 0) { + stracks.erase(tid); + } + } + + std::vector res; + std::map::iterator it; + for (it = stracks.begin(); it != stracks.end(); ++it) { + res.push_back(it->second); + } + + return res; +} + +void ByteTracker::remove_duplicate_stracks( + std::vector & resa, std::vector & resb, std::vector & stracksa, + std::vector & stracksb) +{ + std::vector> pdist = iou_distance(stracksa, stracksb); + std::vector> pairs; + for (size_t i = 0; i < pdist.size(); i++) { + for (size_t j = 0; j < pdist[i].size(); j++) { + if (pdist[i][j] < 0.15) { + pairs.push_back(std::pair(i, j)); + } + } + } + + std::vector dupa, dupb; + for (size_t i = 0; i < pairs.size(); i++) { + int timep = stracksa[pairs[i].first].frame_id - stracksa[pairs[i].first].start_frame; + int timeq = stracksb[pairs[i].second].frame_id - stracksb[pairs[i].second].start_frame; + if (timep > timeq) + dupb.push_back(pairs[i].second); + else + dupa.push_back(pairs[i].first); + } + + for (size_t i = 0; i < stracksa.size(); i++) { + std::vector::iterator iter = find(dupa.begin(), dupa.end(), i); + if (iter == dupa.end()) { + resa.push_back(stracksa[i]); + } + } + + for (size_t i = 0; i < stracksb.size(); i++) { + std::vector::iterator iter = find(dupb.begin(), dupb.end(), i); + if (iter == dupb.end()) { + resb.push_back(stracksb[i]); + } + } +} + +void ByteTracker::linear_assignment( + std::vector> & cost_matrix, int cost_matrix_size, int cost_matrix_size_size, + float thresh, std::vector> & matches, std::vector & unmatched_a, + std::vector & unmatched_b) +{ + if (cost_matrix.size() == 0) { + for (int i = 0; i < cost_matrix_size; i++) { + unmatched_a.push_back(i); + } + for (int i = 0; i < cost_matrix_size_size; i++) { + unmatched_b.push_back(i); + } + return; + } + + std::vector rowsol; + std::vector colsol; + [[maybe_unused]] float c = lapjv(cost_matrix, rowsol, colsol, true, thresh); + for (size_t i = 0; i < rowsol.size(); i++) { + if (rowsol[i] >= 0) { + std::vector match; + match.push_back(i); + match.push_back(rowsol[i]); + matches.push_back(match); + } else { + unmatched_a.push_back(i); + } + } + + for (size_t i = 0; i < colsol.size(); i++) { + if (colsol[i] < 0) { + unmatched_b.push_back(i); + } + } +} + +std::vector> ByteTracker::ious( + std::vector> & atlbrs, std::vector> & btlbrs) +{ + std::vector> ious; + if (atlbrs.size() * btlbrs.size() == 0) return ious; + + ious.resize(atlbrs.size()); + for (size_t i = 0; i < ious.size(); i++) { + ious[i].resize(btlbrs.size()); + } + + // bbox_ious + for (size_t k = 0; k < btlbrs.size(); k++) { + float box_area = (btlbrs[k][2] - btlbrs[k][0] + 1) * (btlbrs[k][3] - btlbrs[k][1] + 1); + for (size_t n = 0; n < atlbrs.size(); n++) { + float iw = std::min(atlbrs[n][2], btlbrs[k][2]) - std::max(atlbrs[n][0], btlbrs[k][0]) + 1; + if (iw > 0) { + float ih = std::min(atlbrs[n][3], btlbrs[k][3]) - std::max(atlbrs[n][1], btlbrs[k][1]) + 1; + if (ih > 0) { + float ua = (atlbrs[n][2] - atlbrs[n][0] + 1) * (atlbrs[n][3] - atlbrs[n][1] + 1) + + box_area - iw * ih; + ious[n][k] = iw * ih / ua; + } else { + ious[n][k] = 0.0; + } + } else { + ious[n][k] = 0.0; + } + } + } + + return ious; +} + +std::vector> ByteTracker::iou_distance( + std::vector & atracks, std::vector & btracks, int & dist_size, + int & dist_size_size) +{ + std::vector> cost_matrix; + if (atracks.size() * btracks.size() == 0) { + dist_size = atracks.size(); + dist_size_size = btracks.size(); + return cost_matrix; + } + std::vector> atlbrs, btlbrs; + for (size_t i = 0; i < atracks.size(); i++) { + atlbrs.push_back(atracks[i]->tlbr); + } + for (size_t i = 0; i < btracks.size(); i++) { + btlbrs.push_back(btracks[i].tlbr); + } + + dist_size = atracks.size(); + dist_size_size = btracks.size(); + + std::vector> _ious = ious(atlbrs, btlbrs); + + for (size_t i = 0; i < _ious.size(); i++) { + std::vector _iou; + for (size_t j = 0; j < _ious[i].size(); j++) { + _iou.push_back(1 - _ious[i][j]); + } + cost_matrix.push_back(_iou); + } + + return cost_matrix; +} + +std::vector> ByteTracker::iou_distance( + std::vector & atracks, std::vector & btracks) +{ + std::vector> atlbrs, btlbrs; + for (size_t i = 0; i < atracks.size(); i++) { + atlbrs.push_back(atracks[i].tlbr); + } + for (size_t i = 0; i < btracks.size(); i++) { + btlbrs.push_back(btracks[i].tlbr); + } + + std::vector> _ious = ious(atlbrs, btlbrs); + std::vector> cost_matrix; + for (size_t i = 0; i < _ious.size(); i++) { + std::vector _iou; + for (size_t j = 0; j < _ious[i].size(); j++) { + _iou.push_back(1 - _ious[i][j]); + } + cost_matrix.push_back(_iou); + } + + return cost_matrix; +} + +double ByteTracker::lapjv( + const std::vector> & cost, std::vector & rowsol, + std::vector & colsol, bool extend_cost, float cost_limit, bool return_cost) +{ + std::vector> cost_c; + cost_c.assign(cost.begin(), cost.end()); + + int n_rows = cost.size(); + int n_cols = cost[0].size(); + rowsol.resize(n_rows); + colsol.resize(n_cols); + + int n = 0; + if (n_rows == n_cols) { + n = n_rows; + } else { + if (!extend_cost) { + std::cout << "set extend_cost=True" << std::endl; + // system("pause"); + exit(0); + } + } + + if (extend_cost || cost_limit < std::numeric_limits::max()) { + std::vector> cost_c_extended; + + n = n_rows + n_cols; + cost_c_extended.resize(n); + for (size_t i = 0; i < cost_c_extended.size(); i++) cost_c_extended[i].resize(n); + + if (cost_limit < std::numeric_limits::max()) { + for (size_t i = 0; i < cost_c_extended.size(); i++) { + for (size_t j = 0; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = cost_limit / 2.0; + } + } + } else { + float cost_max = -1; + for (size_t i = 0; i < cost_c.size(); i++) { + for (size_t j = 0; j < cost_c[i].size(); j++) { + if (cost_c[i][j] > cost_max) cost_max = cost_c[i][j]; + } + } + for (size_t i = 0; i < cost_c_extended.size(); i++) { + for (size_t j = 0; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = cost_max + 1; + } + } + } + + for (size_t i = n_rows; i < cost_c_extended.size(); i++) { + for (size_t j = n_cols; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = 0; + } + } + for (int i = 0; i < n_rows; i++) { + for (int j = 0; j < n_cols; j++) { + cost_c_extended[i][j] = cost_c[i][j]; + } + } + + cost_c.clear(); + cost_c.assign(cost_c_extended.begin(), cost_c_extended.end()); + } + + double ** cost_ptr; + cost_ptr = new double *[sizeof(double *) * n]; + for (int i = 0; i < n; i++) cost_ptr[i] = new double[sizeof(double) * n]; + + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + cost_ptr[i][j] = cost_c[i][j]; + } + } + + int * x_c = new int[sizeof(int) * n]; + int * y_c = new int[sizeof(int) * n]; + + int ret = lapjv_internal(n, cost_ptr, x_c, y_c); + if (ret != 0) { + std::cout << "Calculate Wrong!" << std::endl; + // system("pause"); + exit(0); + } + + double opt = 0.0; + + if (n != n_rows) { + for (int i = 0; i < n; i++) { + if (x_c[i] >= n_cols) x_c[i] = -1; + if (y_c[i] >= n_rows) y_c[i] = -1; + } + for (int i = 0; i < n_rows; i++) { + rowsol[i] = x_c[i]; + } + for (int i = 0; i < n_cols; i++) { + colsol[i] = y_c[i]; + } + + if (return_cost) { + for (size_t i = 0; i < rowsol.size(); i++) { + if (rowsol[i] != -1) { + opt += cost_ptr[i][rowsol[i]]; + } + } + } + } else if (return_cost) { + for (size_t i = 0; i < rowsol.size(); i++) { + opt += cost_ptr[i][rowsol[i]]; + } + } + + for (int i = 0; i < n; i++) { + delete[] cost_ptr[i]; + } + delete[] cost_ptr; + delete[] x_c; + delete[] y_c; + + return opt; +} + +cv::Scalar ByteTracker::get_color(int idx) +{ + idx += 3; + return cv::Scalar(37 * idx % 255, 17 * idx % 255, 29 * idx % 255); +} diff --git a/perception/autoware_bytetrack/src/bytetrack.cpp b/perception/autoware_bytetrack/src/bytetrack.cpp index 0edeb0ac4e247..af93034972a83 100644 --- a/perception/autoware_bytetrack/src/bytetrack.cpp +++ b/perception/autoware_bytetrack/src/bytetrack.cpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include namespace autoware::bytetrack { diff --git a/perception/autoware_bytetrack/src/bytetrack_node.cpp b/perception/autoware_bytetrack/src/bytetrack_node.cpp index 1da7ebc4055f0..bee93b623ae06 100644 --- a/perception/autoware_bytetrack/src/bytetrack_node.cpp +++ b/perception/autoware_bytetrack/src/bytetrack_node.cpp @@ -20,6 +20,8 @@ #include +#include +#include #include #include diff --git a/perception/autoware_bytetrack/src/bytetrack_visualizer_node.cpp b/perception/autoware_bytetrack/src/bytetrack_visualizer_node.cpp index 4276b86f5cab7..a6fc463dd80f1 100644 --- a/perception/autoware_bytetrack/src/bytetrack_visualizer_node.cpp +++ b/perception/autoware_bytetrack/src/bytetrack_visualizer_node.cpp @@ -17,6 +17,10 @@ #include #include +#include +#include +#include + #if __has_include() #include #else @@ -34,7 +38,7 @@ ByteTrackVisualizerNode::ByteTrackVisualizerNode(const rclcpp::NodeOptions & nod { using std::chrono_literals::operator""ms; - use_raw_ = declare_parameter("use_raw", false); + use_raw_ = declare_parameter("use_raw"); // Create timer to find proper settings for subscribed topics timer_ = rclcpp::create_timer( diff --git a/perception/autoware_cluster_merger/test/test_cluster_merger.cpp b/perception/autoware_cluster_merger/test/test_cluster_merger.cpp index d530832659a2f..640bac0afe5fa 100644 --- a/perception/autoware_cluster_merger/test/test_cluster_merger.cpp +++ b/perception/autoware_cluster_merger/test/test_cluster_merger.cpp @@ -19,6 +19,8 @@ #include +#include +#include #include using autoware::cluster_merger::ClusterMergerNode; diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp index a2e3afd62cdad..ae07d54ad53d6 100644 --- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::compare_map_segmentation diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp index d6b65daa82267..33c9d9ff3c788 100644 --- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp +++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp @@ -28,14 +28,14 @@ namespace autoware::compare_map_segmentation { -using PointCloud = typename pcl::Filter::PointCloud; -using PointCloudPtr = typename PointCloud::Ptr; -using PointCloudConstPtr = typename PointCloud::ConstPtr; +using FilteredPointCloud = typename pcl::Filter::PointCloud; +using FilteredPointCloudPtr = typename FilteredPointCloud::Ptr; +using FilteredPointCloudConstPtr = typename FilteredPointCloud::ConstPtr; class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader { private: - PointCloudConstPtr map_ptr_; + FilteredPointCloudConstPtr map_ptr_; pcl::search::Search::Ptr tree_; public: diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index a02a6865ef9ef..9f325b36676be 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::compare_map_segmentation diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index cdad52b5a278d..a31d5ec5dd6d6 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::compare_map_segmentation diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp index 291a19d02ed4b..d0bcf381cd62f 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::compare_map_segmentation diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp index 218f19601cd41..9fb81aa675655 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp @@ -29,14 +29,14 @@ namespace autoware::compare_map_segmentation { -using PointCloud = typename pcl::Filter::PointCloud; -using PointCloudPtr = typename PointCloud::Ptr; -using PointCloudConstPtr = typename PointCloud::ConstPtr; +using FilteredPointCloud = typename pcl::Filter::PointCloud; +using FilteredPointCloudPtr = typename FilteredPointCloud::Ptr; +using FilteredPointCloudConstPtr = typename FilteredPointCloud::ConstPtr; class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader { private: - PointCloudConstPtr map_ptr_; + FilteredPointCloudConstPtr map_ptr_; pcl::search::Search::Ptr tree_; public: @@ -55,7 +55,7 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader { protected: private: - PointCloudConstPtr map_ptr_; + FilteredPointCloudConstPtr map_ptr_; /* data */ public: explicit VoxelDistanceBasedDynamicMapLoader( @@ -78,7 +78,7 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); VoxelGridPointXYZ map_cell_voxel_grid_tmp; - PointCloudPtr map_cell_downsampled_pc_ptr_tmp; + FilteredPointCloudPtr map_cell_downsampled_pc_ptr_tmp; auto map_cell_voxel_input_tmp_ptr = std::make_shared>(map_cell_pc_tmp); diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp index b57874d7d3b24..e93b2097e583e 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp @@ -14,6 +14,10 @@ #include "voxel_grid_map_loader.hpp" +#include +#include +#include + namespace autoware::compare_map_segmentation { VoxelGridMapLoader::VoxelGridMapLoader( @@ -59,7 +63,7 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( } bool VoxelGridMapLoader::is_close_to_neighbor_voxels( - const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map, + const pcl::PointXYZ & point, const double distance_threshold, const FilteredPointCloudPtr & map, VoxelGridPointXYZ & voxel) const { // check map downsampled pc @@ -224,7 +228,8 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( bool VoxelGridMapLoader::is_in_voxel( const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point, - const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const + const double distance_threshold, const FilteredPointCloudPtr & map, + VoxelGridPointXYZ & voxel) const { int voxel_index = voxel.getCentroidIndexAt(voxel.getGridCoordinates(src_point.x, src_point.y, src_point.z)); diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 7faabf2266291..3685008ffc4fe 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -54,9 +54,9 @@ class VoxelGridEx : public pcl::VoxelGrid using pcl::VoxelGrid::div_b_; using pcl::VoxelGrid::inverse_leaf_size_; - using PointCloud = typename pcl::Filter::PointCloud; - using PointCloudPtr = typename PointCloud::Ptr; - using PointCloudConstPtr = typename PointCloud::ConstPtr; + using FilteredPointCloud = typename pcl::Filter::PointCloud; + using FilteredPointCloudPtr = typename FilteredPointCloud::Ptr; + using FilteredPointCloudConstPtr = typename FilteredPointCloud::ConstPtr; public: using pcl::VoxelGrid::leaf_layout_; @@ -93,8 +93,8 @@ class VoxelGridMapLoader public: using VoxelGridPointXYZ = VoxelGridEx; - using PointCloud = typename pcl::Filter::PointCloud; - using PointCloudPtr = typename PointCloud::Ptr; + using FilteredPointCloud = typename pcl::Filter::PointCloud; + using FilteredPointCloudPtr = typename FilteredPointCloud::Ptr; explicit VoxelGridMapLoader( rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, std::string * tf_map_input_frame); @@ -106,11 +106,12 @@ class VoxelGridMapLoader const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel, pcl::search::Search::Ptr tree); bool is_close_to_neighbor_voxels( - const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map, + const pcl::PointXYZ & point, const double distance_threshold, const FilteredPointCloudPtr & map, VoxelGridPointXYZ & voxel) const; bool is_in_voxel( const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point, - const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const; + const double distance_threshold, const FilteredPointCloudPtr & map, + VoxelGridPointXYZ & voxel) const; void publish_downsampled_map(const pcl::PointCloud & downsampled_pc); std::string * tf_map_input_frame_; @@ -121,7 +122,7 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader protected: rclcpp::Subscription::SharedPtr sub_map_; VoxelGridPointXYZ voxel_grid_; - PointCloudPtr voxel_map_ptr_; + FilteredPointCloudPtr voxel_map_ptr_; std::atomic_bool is_initialized_{false}; public: @@ -138,7 +139,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader struct MapGridVoxelInfo { VoxelGridPointXYZ map_cell_voxel_grid; - PointCloudPtr map_cell_pc_ptr; + FilteredPointCloudPtr map_cell_pc_ptr; float min_b_x, min_b_y, max_b_x, max_b_y; pcl::search::Search::Ptr map_cell_kdtree; }; @@ -291,7 +292,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); VoxelGridPointXYZ map_cell_voxel_grid_tmp; - PointCloudPtr map_cell_downsampled_pc_ptr_tmp; + FilteredPointCloudPtr map_cell_downsampled_pc_ptr_tmp; auto map_cell_voxel_input_tmp_ptr = std::make_shared>(map_cell_pc_tmp); diff --git a/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp index 2ed66b9875ef8..683ee8f129d95 100644 --- a/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp +++ b/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp @@ -23,6 +23,9 @@ #include +#include +#include + using autoware::compare_map_segmentation::DistanceBasedCompareMapFilterComponent; using autoware::point_types::PointXYZIRC; using autoware::point_types::PointXYZIRCGenerator; diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp index 816ba7e8174c7..055a0a669795b 100644 --- a/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp @@ -23,6 +23,9 @@ #include +#include +#include + using autoware::compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent; using autoware::point_types::PointXYZIRC; using autoware::point_types::PointXYZIRCGenerator; diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp index 4fa0ab6321f1c..24b06ef917e65 100644 --- a/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp @@ -23,6 +23,9 @@ #include +#include +#include + using autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent; using autoware::point_types::PointXYZIRC; using autoware::point_types::PointXYZIRCGenerator; diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp index 946eef9bd0a0e..c478eb54e9808 100644 --- a/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp @@ -23,6 +23,9 @@ #include +#include +#include + using autoware::compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent; using autoware::point_types::PointXYZIRC; using autoware::point_types::PointXYZIRCGenerator; diff --git a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp index 13f31d92b99a7..bfa8d7c064c55 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp @@ -16,8 +16,11 @@ #include #include +#include #include #include +#include +#include #include #include diff --git a/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.cpp b/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.cpp index 47fe67de9d9aa..21e069144203c 100644 --- a/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.cpp +++ b/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.cpp @@ -14,6 +14,8 @@ #include "detected_object_feature_remover_node.hpp" +#include + namespace autoware::detected_object_feature_remover { DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOptions & node_options) diff --git a/perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp b/perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp index e54151786278a..c180387ecd63e 100644 --- a/perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp +++ b/perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp @@ -21,6 +21,9 @@ #include +#include +#include + namespace { using autoware::detected_object_feature_remover::DetectedObjectFeatureRemover; diff --git a/perception/autoware_detected_object_validation/CMakeLists.txt b/perception/autoware_detected_object_validation/CMakeLists.txt index 434c258918493..bf205fb32bb1d 100644 --- a/perception/autoware_detected_object_validation/CMakeLists.txt +++ b/perception/autoware_detected_object_validation/CMakeLists.txt @@ -48,6 +48,7 @@ target_link_libraries(obstacle_pointcloud_based_validator ) ament_auto_add_library(object_lanelet_filter SHARED + src/lanelet_filter/debug.cpp src/lanelet_filter/lanelet_filter.cpp lib/utils/utils.cpp ) diff --git a/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml index 99050d9738ae6..d15b2c81cf6db 100644 --- a/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml +++ b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml @@ -19,3 +19,5 @@ enabled: false velocity_yaw_threshold: 0.785398 # [rad] (45 deg) object_speed_threshold: 3.0 # [m/s] + debug: false + lanelet_extra_margin: 0.0 diff --git a/perception/autoware_detected_object_validation/object-lanelet-filter.md b/perception/autoware_detected_object_validation/object-lanelet-filter.md index a14e28a42666d..b748885c188b4 100644 --- a/perception/autoware_detected_object_validation/object-lanelet-filter.md +++ b/perception/autoware_detected_object_validation/object-lanelet-filter.md @@ -24,6 +24,11 @@ The objects only inside of the vector map will be published. ## Parameters +| Name | Type | Description | +| ---------------------- | -------- | ------------------------------------------------------------------------------------ | +| `debug` | `bool` | if true, publish debug markers | +| `lanelet_extra_margin` | `double` | `if > 0` lanelet polygons are expanded by extra margin, `if <= 0` margin is disabled | + ### Core Parameters | Name | Type | Default Value | Description | diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/debug.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/debug.cpp new file mode 100644 index 0000000000000..b3765a6a45870 --- /dev/null +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/debug.cpp @@ -0,0 +1,130 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lanelet_filter.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::detected_object_validation +{ +namespace lanelet_filter +{ + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +template +std::optional createPolygonMarker( + T polygon, rclcpp::Time stamp, std::string_view ns, size_t marker_id, + std_msgs::msg::ColorRGBA color) +{ + if (polygon.empty()) { + return std::nullopt; + } + const constexpr std::string_view FRAME_ID = "map"; + + auto create_marker = [&](auto marker_type) { + Marker marker; + marker.id = marker_id; + marker.header.frame_id = FRAME_ID; + marker.header.stamp = stamp; + marker.ns = ns; + marker.type = marker_type; + marker.action = Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + return marker; + }; + + auto marker = create_marker(Marker::LINE_LIST); + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.1; + marker.color.a = color.a; + marker.color.r = color.r; + marker.color.g = color.g; + marker.color.b = color.b; + + std::vector points; + points.reserve(polygon.size() * 2); + + for (auto it = polygon.begin(); it != std::prev(polygon.end()); ++it) { + geometry_msgs::msg::Point point; + + point.x = it->x(); + point.y = it->y(); + points.push_back(point); + + const auto next = std::next(it); + point.x = next->x(); + point.y = next->y(); + points.push_back(point); + } + geometry_msgs::msg::Point point; + point.x = polygon.back().x(); + point.y = polygon.back().y(); + + points.push_back(point); + point.x = polygon.front().x(); + point.y = polygon.front().y(); + + points.push_back(point); + marker.points = points; + return marker; +} + +void ObjectLaneletFilterNode::publishDebugMarkers( + rclcpp::Time stamp, const LinearRing2d & hull, const std::vector & lanelets) +{ + using visualization_msgs::msg::Marker; + using visualization_msgs::msg::MarkerArray; + + uint8_t marker_id = 0; + Marker delete_marker; + constexpr std::string_view lanelet_range = "lanelet_range"; + constexpr std::string_view roi = "roi"; + + MarkerArray marker_array; + + std_msgs::msg::ColorRGBA color; + color.a = 0.5f; + color.r = 0.3f; + color.g = 1.0f; + color.b = 0.2f; + if (auto marker = createPolygonMarker(hull, stamp, lanelet_range, ++marker_id, color); marker) { + marker_array.markers.push_back(std::move(*marker)); + } + for (const auto & box_and_lanelet : lanelets) { + color.r = 0.2; + color.g = 0.5; + color.b = 1.0; + auto p = box_and_lanelet.second.polygon; + if (auto marker = createPolygonMarker(p, stamp, roi, ++marker_id, color); marker) { + marker_array.markers.push_back(std::move(*marker)); + } + } + viz_pub_->publish(marker_array); +} +} // namespace lanelet_filter +} // namespace autoware::detected_object_validation diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index e54ffd5452e7c..f91c4e2036227 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -30,6 +30,10 @@ #include #include +#include +#include +#include + namespace autoware::detected_object_validation { namespace lanelet_filter @@ -59,6 +63,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod declare_parameter("filter_settings.lanelet_direction_filter.velocity_yaw_threshold"); filter_settings_.lanelet_direction_filter_object_speed_threshold = declare_parameter("filter_settings.lanelet_direction_filter.object_speed_threshold"); + filter_settings_.debug = declare_parameter("filter_settings.debug"); + filter_settings_.lanelet_extra_margin = + declare_parameter("filter_settings.lanelet_extra_margin"); // Set publisher/subscriber map_sub_ = this->create_subscription( @@ -73,6 +80,41 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod std::make_unique(this, "object_lanelet_filter"); published_time_publisher_ = std::make_unique(this); + stop_watch_ptr_ = + std::make_unique>(); + if (filter_settings_.debug) { + viz_pub_ = this->create_publisher( + "~/debug/marker", rclcpp::QoS{1}); + } +} + +bool isInPolygon( + const geometry_msgs::msg::Pose & current_pose, const lanelet::BasicPolygon2d & polygon, + const double radius) +{ + constexpr double eps = 1.0e-9; + const lanelet::BasicPoint2d p(current_pose.position.x, current_pose.position.y); + return boost::geometry::distance(p, polygon) < radius + eps; +} + +LinearRing2d expandPolygon(const LinearRing2d & polygon, double distance) +{ + universe_utils::MultiPolygon2d multi_polygon; + bg::strategy::buffer::distance_symmetric distance_strategy(distance); + bg::strategy::buffer::join_miter join_strategy; + bg::strategy::buffer::end_flat end_strategy; + bg::strategy::buffer::point_circle circle_strategy; + bg::strategy::buffer::side_straight side_strategy; + + bg::buffer( + polygon, multi_polygon, distance_strategy, side_strategy, join_strategy, end_strategy, + circle_strategy); + + if (multi_polygon.empty()) { + return polygon; + } + + return multi_polygon.front().outer(); } void ObjectLaneletFilterNode::mapCallback( @@ -86,6 +128,8 @@ void ObjectLaneletFilterNode::mapCallback( void ObjectLaneletFilterNode::objectCallback( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_msg) { + stop_watch_ptr_->tic("processing_time"); + // Guard if (object_pub_->get_subscription_count() < 1) return; @@ -116,6 +160,9 @@ void ObjectLaneletFilterNode::objectCallback( local_rtree.insert(bbox_and_lanelet); } + if (filter_settings_.debug) { + publishDebugMarkers(input_msg->header.stamp, convex_hull, intersected_lanelets_with_bbox); + } // filtering process for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { const auto & transformed_object = transformed_objects.objects.at(index); @@ -135,6 +182,8 @@ void ObjectLaneletFilterNode::objectCallback( .count(); debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency); + debug_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } bool ObjectLaneletFilterNode::filterObject( @@ -217,6 +266,9 @@ LinearRing2d ObjectLaneletFilterNode::getConvexHull( LinearRing2d convex_hull; bg::convex_hull(candidate_points, convex_hull); + if (filter_settings_.lanelet_extra_margin > 0) { + return expandPolygon(convex_hull, filter_settings_.lanelet_extra_margin); + } return convex_hull; } @@ -265,12 +317,12 @@ std::vector ObjectLaneletFilterNode::getIntersectedLanelets( lanelet.attribute(lanelet::AttributeName::Subtype).value() == "road_shoulder")) { if (bg::intersects(convex_hull, lanelet.polygon2d().basicPolygon())) { // create bbox using boost for making the R-tree in later phase - lanelet::BoundingBox2d lanelet_bbox = lanelet::geometry::boundingBox2d(lanelet); - Point2d min_corner(lanelet_bbox.min().x(), lanelet_bbox.min().y()); - Point2d max_corner(lanelet_bbox.max().x(), lanelet_bbox.max().y()); - Box boost_bbox(min_corner, max_corner); + auto polygon = getPolygon(lanelet); + Box boost_bbox; + bg::envelope(polygon, boost_bbox); - intersected_lanelets_with_bbox.emplace_back(std::make_pair(boost_bbox, lanelet)); + intersected_lanelets_with_bbox.emplace_back( + std::make_pair(boost_bbox, PolygonAndLanelet{polygon, lanelet})); } } } @@ -278,6 +330,25 @@ std::vector ObjectLaneletFilterNode::getIntersectedLanelets( return intersected_lanelets_with_bbox; } +lanelet::BasicPolygon2d ObjectLaneletFilterNode::getPolygon(const lanelet::ConstLanelet & lanelet) +{ + if (filter_settings_.lanelet_extra_margin <= 0) { + return lanelet.polygon2d().basicPolygon(); + } + + auto lanelet_polygon = lanelet.polygon2d().basicPolygon(); + Polygon2d polygon; + bg::assign_points(polygon, lanelet_polygon); + + bg::correct(polygon); + auto polygon_result = expandPolygon(polygon.outer(), filter_settings_.lanelet_extra_margin); + lanelet::BasicPolygon2d result; + + bg::assign_points(result, polygon_result); + + return result; +} + bool ObjectLaneletFilterNode::isObjectOverlapLanelets( const autoware_perception_msgs::msg::DetectedObject & object, const bgi::rtree & local_rtree) @@ -314,7 +385,7 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( point2d.position.y = point_transformed.y; for (const auto & candidate : candidates) { - if (lanelet::utils::isInLanelet(point2d, candidate.second, 0.0)) { + if (isInPolygon(point2d, candidate.second.polygon, 0.0)) { return true; } } @@ -334,7 +405,7 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( local_rtree.query(bgi::intersects(bbox_of_convex_hull), std::back_inserter(candidates)); for (const auto & box_and_lanelet : candidates) { - if (!bg::disjoint(polygon, box_and_lanelet.second.polygon2d().basicPolygon())) { + if (!bg::disjoint(polygon, box_and_lanelet.second.polygon)) { return true; } } @@ -374,14 +445,14 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( local_rtree.query(bgi::intersects(bbox), std::back_inserter(candidates)); for (const auto & box_and_lanelet : candidates) { - const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.kinematics.pose_with_covariance.pose, box_and_lanelet.second, 0.0); + const bool is_in_lanelet = + isInPolygon(object.kinematics.pose_with_covariance.pose, box_and_lanelet.second.polygon, 0.0); if (!is_in_lanelet) { continue; } const double lane_yaw = lanelet::utils::getLaneletAngle( - box_and_lanelet.second, object.kinematics.pose_with_covariance.pose.position); + box_and_lanelet.second.lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_velocity_yaw - lane_yaw; const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index 05020336bc759..724e60df27e49 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -19,12 +19,14 @@ #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_lanelet2_extension/utility/utilities.hpp" #include #include "autoware_map_msgs/msg/lanelet_map_bin.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" +#include #include @@ -50,7 +52,13 @@ namespace bg = boost::geometry; namespace bgi = boost::geometry::index; using Point2d = bg::model::point; using Box = boost::geometry::model::box; -using BoxAndLanelet = std::pair; + +struct PolygonAndLanelet +{ + lanelet::BasicPolygon2d polygon; + lanelet::ConstLanelet lanelet; +}; +using BoxAndLanelet = std::pair; using RtreeAlgo = bgi::rstar<16>; class ObjectLaneletFilterNode : public rclcpp::Node @@ -62,10 +70,16 @@ class ObjectLaneletFilterNode : public rclcpp::Node void objectCallback(const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr); void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr); + void publishDebugMarkers( + rclcpp::Time stamp, const LinearRing2d & hull, const std::vector & lanelets); + rclcpp::Publisher::SharedPtr object_pub_; + rclcpp::Publisher::SharedPtr viz_pub_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; + std::unique_ptr debug_publisher_{nullptr}; + std::unique_ptr> stop_watch_ptr_; lanelet::LaneletMapPtr lanelet_map_ptr_; std::string lanelet_frame_id_; @@ -80,6 +94,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node bool lanelet_direction_filter; double lanelet_direction_filter_velocity_yaw_threshold; double lanelet_direction_filter_object_speed_threshold; + bool debug; + double lanelet_extra_margin; } filter_settings_; bool filterObject( @@ -101,6 +117,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node const bgi::rtree & local_rtree); geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &); + lanelet::BasicPolygon2d getPolygon(const lanelet::ConstLanelet & lanelet); std::unique_ptr published_time_publisher_; }; diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index 090ca795289fa..194f333acd87e 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -21,6 +21,9 @@ #include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp b/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp index 3b88628aa6d84..8c1db64c61d51 100644 --- a/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp +++ b/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp @@ -14,6 +14,8 @@ #include "position_filter.hpp" +#include + namespace autoware::detected_object_validation { namespace position_filter diff --git a/perception/autoware_detected_object_validation/test/object_position_filter/test_object_position_filter.cpp b/perception/autoware_detected_object_validation/test/object_position_filter/test_object_position_filter.cpp index 7a6169254348c..651cec7ae236f 100644 --- a/perception/autoware_detected_object_validation/test/object_position_filter/test_object_position_filter.cpp +++ b/perception/autoware_detected_object_validation/test/object_position_filter/test_object_position_filter.cpp @@ -19,6 +19,8 @@ #include +#include +#include #include using autoware::detected_object_validation::position_filter::ObjectPositionFilterNode; diff --git a/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp index 969c287622757..20a19cee4de59 100644 --- a/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp @@ -148,7 +148,7 @@ void ElevationMapLoaderNode::publish() is_bag_loaded = grid_map::GridMapRosConverter::loadFromBag( *data_manager_.elevation_map_path_, "elevation_map", elevation_map_); } catch (const std::runtime_error & e) { - RCLCPP_ERROR(this->get_logger(), e.what()); + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); is_bag_loaded = false; } if (!is_bag_loaded) { diff --git a/perception/autoware_euclidean_cluster/lib/euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/euclidean_cluster.cpp index 0481b7e1b742d..ce56823894bb2 100644 --- a/perception/autoware_euclidean_cluster/lib/euclidean_cluster.cpp +++ b/perception/autoware_euclidean_cluster/lib/euclidean_cluster.cpp @@ -17,6 +17,8 @@ #include #include +#include + namespace autoware::euclidean_cluster { EuclideanCluster::EuclideanCluster() diff --git a/perception/autoware_euclidean_cluster/lib/utils.cpp b/perception/autoware_euclidean_cluster/lib/utils.cpp index 624c838ef0647..6789c71310a7f 100644 --- a/perception/autoware_euclidean_cluster/lib/utils.cpp +++ b/perception/autoware_euclidean_cluster/lib/utils.cpp @@ -19,6 +19,8 @@ #include #include +#include + namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) diff --git a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index f6e8315a02b77..5f4ef98384d23 100644 --- a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -18,6 +18,7 @@ #include #include +#include namespace autoware::euclidean_cluster { diff --git a/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp index b0b9448dfc0c4..cccb05160942c 100644 --- a/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp @@ -16,6 +16,7 @@ #include "autoware/euclidean_cluster/utils.hpp" +#include #include namespace autoware::euclidean_cluster diff --git a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index e54c55e4873ee..019001c54ba49 100644 --- a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -16,6 +16,7 @@ #include "autoware/euclidean_cluster/utils.hpp" +#include #include namespace autoware::euclidean_cluster diff --git a/perception/autoware_euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp index 759e2b3653868..256cce4841873 100644 --- a/perception/autoware_euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp +++ b/perception/autoware_euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp @@ -23,6 +23,9 @@ #include +#include +#include + using autoware::point_types::PointXYZI; void setPointCloud2Fields(sensor_msgs::msg::PointCloud2 & pointcloud) { diff --git a/perception/autoware_ground_segmentation/CMakeLists.txt b/perception/autoware_ground_segmentation/CMakeLists.txt index 9cf256b9492bb..957f06e8cecc3 100644 --- a/perception/autoware_ground_segmentation/CMakeLists.txt +++ b/perception/autoware_ground_segmentation/CMakeLists.txt @@ -25,6 +25,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/ray_ground_filter/node.cpp src/ransac_ground_filter/node.cpp src/scan_ground_filter/node.cpp + src/scan_ground_filter/grid_ground_filter.cpp ) target_link_libraries(${PROJECT_NAME} diff --git a/perception/autoware_ground_segmentation/config/scan_ground_filter.param.yaml b/perception/autoware_ground_segmentation/config/scan_ground_filter.param.yaml index 49c6c32624e75..ce07e83ecc833 100644 --- a/perception/autoware_ground_segmentation/config/scan_ground_filter.param.yaml +++ b/perception/autoware_ground_segmentation/config/scan_ground_filter.param.yaml @@ -6,7 +6,7 @@ use_virtual_ground_point: True split_height_distance: 0.2 non_ground_height_threshold: 0.20 - grid_size_m: 0.1 + grid_size_m: 0.5 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 4 detection_range_z_max: 2.5 diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp index 83e0538bd56de..5ef05485d36a5 100644 --- a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/data.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/data.hpp new file mode 100644 index 0000000000000..d2624ffb44c97 --- /dev/null +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/data.hpp @@ -0,0 +1,69 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCAN_GROUND_FILTER__DATA_HPP_ +#define SCAN_GROUND_FILTER__DATA_HPP_ + +#include + +#include + +namespace autoware::ground_segmentation +{ +using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; + +class PclDataAccessor +{ +public: + PclDataAccessor() = default; + ~PclDataAccessor() = default; + + bool isInitialized() const { return data_offset_initialized_; } + + void setField(const PointCloud2ConstPtr & input) + { + data_offset_x_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; + data_offset_y_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; + data_offset_z_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + data_offset_intensity_ = input->fields[intensity_index].offset; + intensity_type_ = input->fields[intensity_index].datatype; + } else { + data_offset_intensity_ = -1; + } + data_offset_initialized_ = true; + } + + inline void getPoint( + const PointCloud2ConstPtr & input, const size_t data_index, pcl::PointXYZ & point) const + { + point.x = *reinterpret_cast(&input->data[data_index + data_offset_x_]); + point.y = *reinterpret_cast(&input->data[data_index + data_offset_y_]); + point.z = *reinterpret_cast(&input->data[data_index + data_offset_z_]); + } + +private: + // data field offsets + int data_offset_x_ = 0; + int data_offset_y_ = 0; + int data_offset_z_ = 0; + int data_offset_intensity_ = 0; + int intensity_type_ = 0; + bool data_offset_initialized_ = false; +}; + +} // namespace autoware::ground_segmentation + +#endif // SCAN_GROUND_FILTER__DATA_HPP_ diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp index cd55d4a548211..0dbbe1eabb8da 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp @@ -17,93 +17,485 @@ #include #include +#include +#include #include +#include +#include +#include + +namespace +{ + +float pseudoArcTan2(const float y, const float x) +{ + // lightweight arc tangent + + // avoid divide-by-zero + if (y == 0.0f) { + if (x >= 0.0f) return 0.0f; + return M_PIf; + } + if (x == 0.0f) { + if (y >= 0.0f) return M_PI_2f; + return -M_PI_2f; + } + + const float x_abs = std::abs(x); + const float y_abs = std::abs(y); + + // divide to 8 zones + constexpr float M_2PIf = 2.0f * M_PIf; + constexpr float M_3PI_2f = 3.0f * M_PI_2f; + if (x_abs > y_abs) { + const float ratio = y_abs / x_abs; + const float angle = ratio * M_PI_4f; + if (y >= 0.0f) { + if (x >= 0.0f) return angle; // 1st zone + return M_PIf - angle; // 2nd zone + } else { + if (x >= 0.0f) return M_2PIf - angle; // 4th zone + return M_PIf + angle; // 3rd zone + } + } else { + const float ratio = x_abs / y_abs; + const float angle = ratio * M_PI_4f; + if (y >= 0.0f) { + if (x >= 0.0f) return M_PI_2f - angle; // 1st zone + return M_PI_2f + angle; // 2nd zone + } else { + if (x >= 0.0f) return M_3PI_2f + angle; // 4th zone + return M_3PI_2f - angle; // 3rd zone + } + } +} + +float pseudoTan(const float theta) +{ + // lightweight tangent + + // normalize the angle, range of [-pi/2, pi/2] + float normalized_theta = theta; + while (normalized_theta > M_PI_2f) { + normalized_theta -= M_PIf; + } + while (normalized_theta < -M_PI_2f) { + normalized_theta += M_PIf; + } + + // avoid divide-by-zero + if (normalized_theta == 0.0f) return 0.0f; + + if (std::abs(normalized_theta) <= 1.0f) { + return normalized_theta / M_PI_4f; + } + return std::copysign(M_PI_4f / (M_PI_2f - std::abs(normalized_theta)), normalized_theta); +} +} // namespace namespace autoware::ground_segmentation { +using autoware::universe_utils::ScopedTimeTrack; -class ScanGroundGrid +struct Point +{ + size_t index; + float distance; + float height; +}; + +// Concentric Zone Model (CZM) based polar grid +class Cell { public: - ScanGroundGrid() = default; - ~ScanGroundGrid() = default; + // list of point indices + std::vector point_list_; // point index and distance + + // method to check if the cell is empty + inline bool isEmpty() const { return point_list_.empty(); } + + // index of the cell + int grid_idx_; + int radial_idx_; + int azimuth_idx_; + int next_grid_idx_; + int prev_grid_idx_; + + int scan_grid_root_idx_; + + // geometric properties of the cell + float center_radius_; + float center_azimuth_; + float radial_size_; + float azimuth_size_; + + // ground statistics of the points in the cell + float avg_height_; + float max_height_; + float min_height_; + float avg_radius_; + float gradient_; + float intercept_; + + // process flags + bool is_processed_ = false; + bool is_ground_initialized_ = false; + bool has_ground_ = false; +}; + +class Grid +{ +public: + Grid(const float origin_x, const float origin_y, const float origin_z) + : origin_x_(origin_x), origin_y_(origin_y), origin_z_(origin_z) + { + } + ~Grid() = default; + + void setTimeKeeper(std::shared_ptr time_keeper_ptr) + { + time_keeper_ = std::move(time_keeper_ptr); + } void initialize( - const float grid_size_m, const float grid_mode_switch_radius, const float virtual_lidar_z) + const float grid_dist_size, const float grid_azimuth_size, + const float grid_linearity_switch_radius) { - grid_size_m_ = grid_size_m; - mode_switch_radius_ = grid_mode_switch_radius; - virtual_lidar_z_ = virtual_lidar_z; - - // calculate parameters - inv_grid_size_m_ = 1.0f / grid_size_m_; - mode_switch_grid_id_ = mode_switch_radius_ * inv_grid_size_m_; - mode_switch_angle_rad_ = std::atan2(mode_switch_radius_, virtual_lidar_z_); - - grid_size_rad_ = universe_utils::normalizeRadian( - std::atan2(mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - universe_utils::normalizeRadian(mode_switch_angle_rad_); - inv_grid_size_rad_ = 1.0f / grid_size_rad_; - tan_grid_size_rad_ = std::tan(grid_size_rad_); - grid_id_offset_ = mode_switch_grid_id_ - mode_switch_angle_rad_ * inv_grid_size_rad_; + grid_dist_size_ = grid_dist_size; + grid_azimuth_size_ = grid_azimuth_size; + + // set grid linearity switch radius + grid_linearity_switch_num_ = static_cast(grid_linearity_switch_radius / grid_dist_size_); + grid_linearity_switch_radius_ = grid_linearity_switch_num_ * grid_dist_size_; + + // calculate grid parameters + grid_dist_size_rad_ = + pseudoArcTan2(grid_linearity_switch_radius_ + grid_dist_size_, origin_z_) - + pseudoArcTan2(grid_linearity_switch_radius_, origin_z_); + grid_dist_size_inv_ = 1.0f / grid_dist_size_; + + // generate grid geometry + setGridBoundaries(); + // initialize and resize cells + cells_.clear(); + cells_.resize(radial_idx_offsets_.back() + azimuth_grids_per_radial_.back()); + + // set cell geometry + setCellGeometry(); + + // set initialized flag is_initialized_ = true; } - float getGridSize(const float radius, const size_t grid_id) const + // method to add a point to the grid + void addPoint(const float x, const float y, const float z, const size_t point_idx) { - // check if initialized - if (!is_initialized_) { - throw std::runtime_error("ScanGroundGrid is not initialized."); + const float x_fixed = x - origin_x_; + const float y_fixed = y - origin_y_; + const float radius = std::sqrt(x_fixed * x_fixed + y_fixed * y_fixed); + const float azimuth = pseudoArcTan2(y_fixed, x_fixed); + + // calculate the grid id + const int grid_idx = getGridIdx(radius, azimuth); + + // check if the point is within the grid + if (grid_idx < 0) { + return; } + const size_t grid_idx_idx = static_cast(grid_idx); - float grid_size = grid_size_m_; - constexpr uint16_t back_steps_num = 1; + // add the point to the cell + cells_[grid_idx_idx].point_list_.emplace_back(Point{point_idx, radius, z}); + } + + size_t getGridSize() const { return cells_.size(); } - if (radius > mode_switch_radius_ && grid_id > mode_switch_grid_id_ + back_steps_num) { - // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) * - // virtual_lidar_z_ - // where gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f) - grid_size = radius - (radius - tan_grid_size_rad_ * virtual_lidar_z_) / - (1 + radius * tan_grid_size_rad_ / virtual_lidar_z_); + // method to get the cell + inline Cell & getCell(const int grid_idx) + { + const size_t idx = static_cast(grid_idx); + if (idx >= cells_.size()) { + throw std::out_of_range("Invalid grid index"); } - return grid_size; + return cells_[idx]; } - uint16_t getGridId(const float radius) const + void resetCells() { - // check if initialized - if (!is_initialized_) { - throw std::runtime_error("ScanGroundGrid is not initialized."); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + for (auto & cell : cells_) { + cell.point_list_.clear(); + cell.is_processed_ = false; + cell.is_ground_initialized_ = false; + cell.has_ground_ = false; } + } - uint16_t grid_id = 0; - if (radius <= mode_switch_radius_) { - grid_id = static_cast(radius * inv_grid_size_m_); - } else { - auto gamma{universe_utils::normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; - grid_id = grid_id_offset_ + gamma * inv_grid_size_rad_; + void setGridConnections() + { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + // iterate over grid cells + for (Cell & cell : cells_) { + // find and link the scan-grid root cell + cell.scan_grid_root_idx_ = cell.prev_grid_idx_; + while (cell.scan_grid_root_idx_ >= 0) { + const auto & prev_cell = cells_[cell.scan_grid_root_idx_]; + // if the previous cell has point, set the previous cell as the root cell + if (!prev_cell.isEmpty()) break; + // keep searching the previous cell + cell.scan_grid_root_idx_ = prev_cell.scan_grid_root_idx_; + } + // if the grid root idx reaches -1, finish the search } - return grid_id; } private: + // given parameters + float origin_x_; + float origin_y_; + float origin_z_; + float grid_dist_size_ = 1.0f; // meters + float grid_azimuth_size_ = 0.01f; // radians + float grid_linearity_switch_radius_ = 20.0f; // meters + + // calculated parameters + float grid_dist_size_rad_ = 0.0f; // radians + float grid_dist_size_inv_ = 0.0f; // inverse of the grid size in meters + int grid_linearity_switch_num_ = 0; // number of grids within the switch radius + float grid_linearity_switch_angle_ = 0.0f; // angle at the switch radius + float grid_size_rad_inv_ = 0.0f; // inverse of the grid size in radians bool is_initialized_ = false; // configured parameters - float grid_size_m_ = 0.0f; - float mode_switch_radius_ = 0.0f; - float virtual_lidar_z_ = 0.0f; + float grid_radial_limit_ = 200.0f; // meters - // calculated parameters - float inv_grid_size_m_ = 0.0f; - float grid_size_rad_ = 0.0f; - float inv_grid_size_rad_ = 0.0f; - float tan_grid_size_rad_ = 0.0f; - float mode_switch_grid_id_ = 0.0f; - float mode_switch_angle_rad_ = 0.0f; - float grid_id_offset_ = 0.0f; + // array of grid boundaries + std::vector grid_radial_boundaries_; + std::vector azimuth_grids_per_radial_; + std::vector azimuth_interval_per_radial_; + std::vector radial_idx_offsets_; + + // list of cells + std::vector cells_; + + // debug information + std::shared_ptr time_keeper_; + + // Generate grid geometry + // the grid is cylindrical mesh grid + // azimuth interval: constant angle + // radial interval: constant distance within mode switch radius + // constant elevation angle outside mode switch radius + void setGridBoundaries() + { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + // radial boundaries + { + // constant distance + for (int i = 0; i < grid_linearity_switch_num_; i++) { + grid_radial_boundaries_.push_back(i * grid_dist_size_); + } + // constant angle + grid_linearity_switch_angle_ = pseudoArcTan2(grid_linearity_switch_radius_, origin_z_); + float angle = grid_linearity_switch_angle_; + const float grid_angle_interval = + pseudoArcTan2(grid_linearity_switch_radius_ + grid_dist_size_, origin_z_) - angle; + grid_size_rad_inv_ = 1.0f / grid_angle_interval; + while (angle < M_PI_2) { + const float dist = pseudoTan(angle) * origin_z_; + grid_radial_boundaries_.push_back(dist); + if (dist > grid_radial_limit_) { + break; + } + angle += grid_angle_interval; + } + } + + const size_t radial_grid_num = grid_radial_boundaries_.size(); + + // azimuth boundaries + { + if (grid_azimuth_size_ <= 0) { + throw std::runtime_error("Grid azimuth size is not positive."); + } + + // number of azimuth grids per radial grid + azimuth_grids_per_radial_.resize(radial_grid_num); + azimuth_interval_per_radial_.resize(radial_grid_num); + azimuth_grids_per_radial_[0] = 1; + azimuth_interval_per_radial_[0] = 2.0f * M_PIf; + + const int max_azimuth_grid_num = static_cast(2.0 * M_PIf / grid_azimuth_size_); + + int divider = 1; + for (size_t i = radial_grid_num - 1; i > 0; --i) { + // set divider + const float radius = grid_radial_boundaries_[i]; + const int divider_next = std::ceil(grid_linearity_switch_radius_ / radius); + if (divider_next % divider == 0 && max_azimuth_grid_num % divider_next == 0) { + divider = divider_next; + } + // set azimuth grid number + const int grid_num = static_cast(max_azimuth_grid_num / divider); + const int azimuth_grid_num = std::max(std::min(grid_num, max_azimuth_grid_num), 1); + const float azimuth_interval_evened = 2.0f * M_PIf / azimuth_grid_num; + + azimuth_grids_per_radial_[i] = azimuth_grid_num; + azimuth_interval_per_radial_[i] = azimuth_interval_evened; + } + } + + // accumulate the number of azimuth grids per radial grid, set offset for each radial grid + radial_idx_offsets_.resize(radial_grid_num); + radial_idx_offsets_[0] = 0; + for (size_t i = 1; i < radial_grid_num; ++i) { + radial_idx_offsets_[i] = radial_idx_offsets_[i - 1] + azimuth_grids_per_radial_[i - 1]; + } + } + + int getAzimuthGridIdx(const int & radial_idx, const float & azimuth) const + { + const int azimuth_grid_num = azimuth_grids_per_radial_[radial_idx]; + + int azimuth_grid_idx = + static_cast(std::floor(azimuth / azimuth_interval_per_radial_[radial_idx])); + if (azimuth_grid_idx == azimuth_grid_num) { + // loop back to the first grid + azimuth_grid_idx = 0; + } + // constant azimuth interval + return azimuth_grid_idx; + } + + int getRadialIdx(const float & radius) const + { + // check if the point is within the grid + if (radius > grid_radial_limit_) { + return -1; + } + if (radius < 0) { + return -1; + } + + // determine the grid id + int grid_rad_idx = -1; + + // constant distance + if (radius < grid_linearity_switch_radius_) { + grid_rad_idx = static_cast(radius * grid_dist_size_inv_); + } else if (radius < grid_radial_limit_) { + const float angle = pseudoArcTan2(radius, origin_z_); + grid_rad_idx = grid_linearity_switch_num_ + + static_cast((angle - grid_linearity_switch_angle_) * grid_size_rad_inv_); + } + + return grid_rad_idx; + } + + int getGridIdx(const int & radial_idx, const int & azimuth_idx) const + { + return radial_idx_offsets_[radial_idx] + azimuth_idx; + } + + // method to determine the grid id of a point + // -1 means out of range + // range limit is horizon angle + int getGridIdx(const float & radius, const float & azimuth) const + { + const int grid_rad_idx = getRadialIdx(radius); + if (grid_rad_idx < 0) { + return -1; + } + + // azimuth grid id + const int grid_az_idx = getAzimuthGridIdx(grid_rad_idx, azimuth); + if (grid_az_idx < 0) { + return -1; + } + + return getGridIdx(grid_rad_idx, grid_az_idx); + } + + void getRadialAzimuthIdxFromCellIdx(const int cell_id, int & radial_idx, int & azimuth_idx) const + { + radial_idx = -1; + azimuth_idx = -1; + for (size_t i = 0; i < radial_idx_offsets_.size(); ++i) { + if (cell_id < radial_idx_offsets_[i]) { + radial_idx = i - 1; + azimuth_idx = cell_id - radial_idx_offsets_[i - 1]; + break; + } + } + if (cell_id >= radial_idx_offsets_.back()) { + radial_idx = radial_idx_offsets_.size() - 1; + azimuth_idx = cell_id - radial_idx_offsets_.back(); + } + } + + void setCellGeometry() + { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + for (size_t idx = 0; idx < cells_.size(); ++idx) { + Cell & cell = cells_[idx]; + + int radial_idx = 0; + int azimuth_idx = 0; + getRadialAzimuthIdxFromCellIdx(idx, radial_idx, azimuth_idx); + + cell.grid_idx_ = idx; + cell.radial_idx_ = radial_idx; + cell.azimuth_idx_ = azimuth_idx; + + // set width of the cell + const auto radial_grid_num = static_cast(grid_radial_boundaries_.size() - 1); + if (radial_idx < radial_grid_num) { + cell.radial_size_ = + grid_radial_boundaries_[radial_idx + 1] - grid_radial_boundaries_[radial_idx]; + } else { + cell.radial_size_ = grid_radial_limit_ - grid_radial_boundaries_[radial_idx]; + } + cell.azimuth_size_ = azimuth_interval_per_radial_[radial_idx]; + + // set center of the cell + cell.center_radius_ = grid_radial_boundaries_[radial_idx] + cell.radial_size_ * 0.5f; + cell.center_azimuth_ = (static_cast(azimuth_idx) + 0.5f) * cell.azimuth_size_; + + // set next grid id, which is radially next + int next_grid_idx = -1; + // only if the next radial grid exists + if (radial_idx < radial_grid_num) { + // find nearest azimuth grid in the next radial grid + const float azimuth = cell.center_azimuth_; + const size_t azimuth_idx_next_radial_grid = getAzimuthGridIdx(radial_idx + 1, azimuth); + next_grid_idx = getGridIdx(radial_idx + 1, azimuth_idx_next_radial_grid); + } + cell.next_grid_idx_ = next_grid_idx; + + // set previous grid id, which is radially previous + int prev_grid_idx = -1; + // only if the previous radial grid exists + if (radial_idx > 0) { + // find nearest azimuth grid in the previous radial grid + const float azimuth = cell.center_azimuth_; + // constant azimuth interval + const size_t azimuth_idx_prev_radial_grid = getAzimuthGridIdx(radial_idx - 1, azimuth); + prev_grid_idx = getGridIdx(radial_idx - 1, azimuth_idx_prev_radial_grid); + } + cell.prev_grid_idx_ = prev_grid_idx; + cell.scan_grid_root_idx_ = -1; + } + } }; } // namespace autoware::ground_segmentation diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.cpp new file mode 100644 index 0000000000000..9c8106f59b917 --- /dev/null +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.cpp @@ -0,0 +1,495 @@ +// 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 "grid_ground_filter.hpp" + +#include "data.hpp" + +#include + +#include +#include + +namespace autoware::ground_segmentation +{ + +// assign the pointcloud data to the grid +void GridGroundFilter::convert() +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + const size_t in_cloud_data_size = in_cloud_->data.size(); + const size_t in_cloud_point_step = in_cloud_->point_step; + + for (size_t data_index = 0; data_index + in_cloud_point_step <= in_cloud_data_size; + data_index += in_cloud_point_step) { + // Get Point + pcl::PointXYZ input_point; + data_accessor_.getPoint(in_cloud_, data_index, input_point); + grid_ptr_->addPoint(input_point.x, input_point.y, input_point.z, data_index); + } +} + +// preprocess the grid data, set the grid connections +void GridGroundFilter::preprocess() +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + // eliminate empty cells from connection for efficiency + grid_ptr_->setGridConnections(); +} + +// recursive search for the ground grid cell close to the grid origin +bool GridGroundFilter::recursiveSearch( + const int check_idx, const int search_cnt, std::vector & idx) const +{ + // set the maximum search count + constexpr size_t count_limit = 1023; + return recursiveSearch(check_idx, search_cnt, idx, count_limit); +} + +bool GridGroundFilter::recursiveSearch( + const int check_idx, const int search_cnt, std::vector & idx, size_t count) const +{ + if (count == 0) { + return false; + } + count -= 1; + // recursive search + if (check_idx < 0) { + return false; + } + if (search_cnt == 0) { + return true; + } + const auto & check_cell = grid_ptr_->getCell(check_idx); + if (check_cell.has_ground_) { + // the cell has ground, add the index to the list, and search previous cell + idx.push_back(check_idx); + return recursiveSearch(check_cell.scan_grid_root_idx_, search_cnt - 1, idx, count); + } + // if the cell does not have ground, search previous cell + return recursiveSearch(check_cell.scan_grid_root_idx_, search_cnt, idx, count); +} + +// fit the line from the ground grid cells +void GridGroundFilter::fitLineFromGndGrid(const std::vector & idx, float & a, float & b) const +{ + // if the idx is empty, the line is not defined + if (idx.empty()) { + a = 0.0f; + b = 0.0f; + return; + } + // if the idx is length of 1, the line is zero-crossing line + if (idx.size() == 1) { + const auto & cell = grid_ptr_->getCell(idx.front()); + a = cell.avg_height_ / cell.avg_radius_; + b = 0.0f; + return; + } + // calculate the line by least square method + float sum_x = 0.0f; + float sum_y = 0.0f; + float sum_xy = 0.0f; + float sum_x2 = 0.0f; + for (const auto & i : idx) { + const auto & cell = grid_ptr_->getCell(i); + sum_x += cell.avg_radius_; + sum_y += cell.avg_height_; + sum_xy += cell.avg_radius_ * cell.avg_height_; + sum_x2 += cell.avg_radius_ * cell.avg_radius_; + } + const float n = static_cast(idx.size()); + const float denominator = n * sum_x2 - sum_x * sum_x; + if (denominator != 0.0f) { + a = (n * sum_xy - sum_x * sum_y) / denominator; + a = std::clamp(a, -param_.global_slope_max_ratio, param_.global_slope_max_ratio); + b = (sum_y - a * sum_x) / n; + } else { + const auto & cell = grid_ptr_->getCell(idx.front()); + a = cell.avg_height_ / cell.avg_radius_; + b = 0.0f; + } +} + +// process the grid data to initialize the ground cells prior to the ground segmentation +void GridGroundFilter::initializeGround(pcl::PointIndices & out_no_ground_indices) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + const auto grid_size = grid_ptr_->getGridSize(); + // loop over grid cells + for (size_t idx = 0; idx < grid_size; idx++) { + auto & cell = grid_ptr_->getCell(idx); + if (cell.is_ground_initialized_) continue; + // if the cell is empty, skip + if (cell.isEmpty()) continue; + + // check scan root grid + if (cell.scan_grid_root_idx_ >= 0) { + const Cell & prev_cell = grid_ptr_->getCell(cell.scan_grid_root_idx_); + if (prev_cell.is_ground_initialized_) { + cell.is_ground_initialized_ = true; + continue; + } + } + + // initialize ground in this cell + bool is_ground_found = false; + PointsCentroid ground_bin; + + for (const auto & pt : cell.point_list_) { + const size_t & pt_idx = pt.index; + const float & radius = pt.distance; + const float & height = pt.height; + + const float global_slope_threshold = param_.global_slope_max_ratio * radius; + if (height >= global_slope_threshold && height > param_.non_ground_height_threshold) { + // this point is obstacle + out_no_ground_indices.indices.push_back(pt_idx); + } else if ( + abs(height) < global_slope_threshold && abs(height) < param_.non_ground_height_threshold) { + // this point is ground + ground_bin.addPoint(radius, height, pt_idx); + is_ground_found = true; + } + // else, this point is not classified, not ground nor obstacle + } + cell.is_processed_ = true; + cell.has_ground_ = is_ground_found; + if (is_ground_found) { + cell.is_ground_initialized_ = true; + ground_bin.processAverage(); + cell.avg_height_ = ground_bin.getAverageHeight(); + cell.avg_radius_ = ground_bin.getAverageRadius(); + cell.max_height_ = ground_bin.getMaxHeight(); + cell.min_height_ = ground_bin.getMinHeight(); + cell.gradient_ = std::clamp( + cell.avg_height_ / cell.avg_radius_, -param_.global_slope_max_ratio, + param_.global_slope_max_ratio); + cell.intercept_ = 0.0f; + } else { + cell.is_ground_initialized_ = false; + } + } +} + +// segment the point in the cell, logic for the continuous cell +void GridGroundFilter::SegmentContinuousCell( + const Cell & cell, PointsCentroid & ground_bin, pcl::PointIndices & out_no_ground_indices) +{ + const Cell & prev_cell = grid_ptr_->getCell(cell.scan_grid_root_idx_); + const float local_thresh_angle_ratio = std::tan(DEG2RAD(5.0)); + + // loop over points in the cell + for (const auto & pt : cell.point_list_) { + const size_t & pt_idx = pt.index; + const float & radius = pt.distance; + const float & height = pt.height; + + // 1. height is out-of-range + const float delta_z = height - prev_cell.avg_height_; + if (delta_z > param_.detection_range_z_max) { + // this point is out-of-range + continue; + } + + // 2. the angle is exceed the global slope threshold + if (height > param_.global_slope_max_ratio * radius) { + // this point is obstacle + out_no_ground_indices.indices.push_back(pt_idx); + // go to the next point + continue; + } + + // 3. local slope + const float delta_radius = radius - prev_cell.avg_radius_; + if (abs(delta_z) < param_.global_slope_max_ratio * delta_radius) { + // this point is ground + ground_bin.addPoint(radius, height, pt_idx); + // go to the next point + continue; + } + + // 3. height from the estimated ground + const float next_gnd_z = cell.gradient_ * radius + cell.intercept_; + const float gnd_z_local_thresh = local_thresh_angle_ratio * delta_radius; + const float delta_gnd_z = height - next_gnd_z; + const float gnd_z_threshold = param_.non_ground_height_threshold + gnd_z_local_thresh; + if (delta_gnd_z > gnd_z_threshold) { + // this point is obstacle + out_no_ground_indices.indices.push_back(pt_idx); + // go to the next point + continue; + } + if (abs(delta_gnd_z) <= gnd_z_threshold) { + // this point is ground + ground_bin.addPoint(radius, height, pt_idx); + // go to the next point + continue; + } + // else, this point is not classified, not ground nor obstacle + } +} + +// segment the point in the cell, logic for the discontinuous cell +void GridGroundFilter::SegmentDiscontinuousCell( + const Cell & cell, PointsCentroid & ground_bin, pcl::PointIndices & out_no_ground_indices) +{ + const Cell & prev_cell = grid_ptr_->getCell(cell.scan_grid_root_idx_); + + // loop over points in the cell + for (const auto & pt : cell.point_list_) { + const size_t & pt_idx = pt.index; + const float & radius = pt.distance; + const float & height = pt.height; + + // 1. height is out-of-range + const float delta_avg_z = height - prev_cell.avg_height_; + if (delta_avg_z > param_.detection_range_z_max) { + // this point is out-of-range + continue; + } + + // 2. the angle is exceed the global slope threshold + if (height > param_.global_slope_max_ratio * radius) { + // this point is obstacle + out_no_ground_indices.indices.push_back(pt_idx); + // go to the next point + continue; + } + // 3. local slope + const float delta_radius = radius - prev_cell.avg_radius_; + const float global_slope_threshold = param_.global_slope_max_ratio * delta_radius; + if (abs(delta_avg_z) < global_slope_threshold) { + // this point is ground + ground_bin.addPoint(radius, height, pt_idx); + // go to the next point + continue; + } + // 4. height from the estimated ground + if (abs(delta_avg_z) < param_.non_ground_height_threshold) { + // this point is ground + ground_bin.addPoint(radius, height, pt_idx); + // go to the next point + continue; + } + const float delta_max_z = height - prev_cell.max_height_; + if (abs(delta_max_z) < param_.non_ground_height_threshold) { + // this point is ground + ground_bin.addPoint(radius, height, pt_idx); + // go to the next point + continue; + } + // 5. obstacle from local slope + if (delta_avg_z >= global_slope_threshold) { + // this point is obstacle + out_no_ground_indices.indices.push_back(pt_idx); + // go to the next point + continue; + } + // else, this point is not classified, not ground nor obstacle + } +} + +// segment the point in the cell, logic for the break cell +void GridGroundFilter::SegmentBreakCell( + const Cell & cell, PointsCentroid & ground_bin, pcl::PointIndices & out_no_ground_indices) +{ + const Cell & prev_cell = grid_ptr_->getCell(cell.scan_grid_root_idx_); + + // loop over points in the cell + for (const auto & pt : cell.point_list_) { + const size_t & pt_idx = pt.index; + const float & radius = pt.distance; + const float & height = pt.height; + + // 1. height is out-of-range + const float delta_z = height - prev_cell.avg_height_; + if (delta_z > param_.detection_range_z_max) { + // this point is out-of-range + continue; + } + + // 2. the angle is exceed the global slope threshold + if (height > param_.global_slope_max_ratio * radius) { + // this point is obstacle + out_no_ground_indices.indices.push_back(pt_idx); + // go to the next point + continue; + } + + // 3. the point is over discontinuous grid + const float delta_radius = radius - prev_cell.avg_radius_; + const float global_slope_threshold = param_.global_slope_max_ratio * delta_radius; + if (abs(delta_z) < global_slope_threshold) { + // this point is ground + ground_bin.addPoint(radius, height, pt_idx); + // go to the next point + continue; + } + if (delta_z >= global_slope_threshold) { + // this point is obstacle + out_no_ground_indices.indices.push_back(pt_idx); + // go to the next point + continue; + } + // else, this point is not classified, not ground nor obstacle + } +} + +// classify the point cloud into ground and non-ground points +void GridGroundFilter::classify(pcl::PointIndices & out_no_ground_indices) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + // loop over grid cells + const auto grid_size = grid_ptr_->getGridSize(); + for (size_t idx = 0; idx < grid_size; idx++) { + auto & cell = grid_ptr_->getCell(idx); + // if the cell is empty, skip + if (cell.isEmpty()) continue; + if (cell.is_processed_) continue; + + // set a cell pointer for the previous cell + // check scan root grid + if (cell.scan_grid_root_idx_ < 0) continue; + const Cell & prev_cell = grid_ptr_->getCell(cell.scan_grid_root_idx_); + if (!(prev_cell.is_ground_initialized_)) continue; + + // get current cell gradient and intercept + std::vector grid_idcs; + { + const int search_count = param_.gnd_grid_buffer_size; + const int check_cell_idx = cell.scan_grid_root_idx_; + recursiveSearch(check_cell_idx, search_count, grid_idcs); + } + + // segment the ground and non-ground points + enum SegmentationMode { NONE, CONTINUOUS, DISCONTINUOUS, BREAK }; + SegmentationMode mode = SegmentationMode::NONE; + { + const int front_radial_id = + grid_ptr_->getCell(grid_idcs.back()).radial_idx_ + grid_idcs.size(); + const float radial_diff_between_cells = cell.center_radius_ - prev_cell.center_radius_; + + if (radial_diff_between_cells < param_.gnd_grid_continual_thresh * cell.radial_size_) { + if (cell.radial_idx_ - front_radial_id < param_.gnd_grid_continual_thresh) { + mode = SegmentationMode::CONTINUOUS; + } else { + mode = SegmentationMode::DISCONTINUOUS; + } + } else { + mode = SegmentationMode::BREAK; + } + } + + { + PointsCentroid ground_bin; + if (mode == SegmentationMode::CONTINUOUS) { + // calculate the gradient and intercept by least square method + float a, b; + fitLineFromGndGrid(grid_idcs, a, b); + cell.gradient_ = a; + cell.intercept_ = b; + + SegmentContinuousCell(cell, ground_bin, out_no_ground_indices); + } else if (mode == SegmentationMode::DISCONTINUOUS) { + SegmentDiscontinuousCell(cell, ground_bin, out_no_ground_indices); + } else if (mode == SegmentationMode::BREAK) { + SegmentBreakCell(cell, ground_bin, out_no_ground_indices); + } + + // recheck ground bin + if ( + param_.use_recheck_ground_cluster && cell.avg_radius_ > param_.grid_mode_switch_radius && + ground_bin.getGroundPointNum() > 0) { + // recheck the ground cluster + float reference_height = 0; + if (param_.use_lowest_point) { + reference_height = ground_bin.getMinHeightOnly(); + } else { + ground_bin.processAverage(); + reference_height = ground_bin.getAverageHeight(); + } + const float threshold = reference_height + param_.non_ground_height_threshold; + const std::vector & gnd_indices = ground_bin.getIndicesRef(); + const std::vector & height_list = ground_bin.getHeightListRef(); + for (size_t j = 0; j < height_list.size(); ++j) { + if (height_list.at(j) >= threshold) { + // fill the non-ground indices + out_no_ground_indices.indices.push_back(gnd_indices.at(j)); + // mark the point as non-ground + ground_bin.is_ground_list.at(j) = false; + } + } + } + + // finalize current cell, update the cell ground information + if (ground_bin.getGroundPointNum() > 0) { + ground_bin.processAverage(); + cell.avg_height_ = ground_bin.getAverageHeight(); + cell.avg_radius_ = ground_bin.getAverageRadius(); + cell.max_height_ = ground_bin.getMaxHeight(); + cell.min_height_ = ground_bin.getMinHeight(); + cell.has_ground_ = true; + } else { + // copy previous cell + cell.avg_radius_ = prev_cell.avg_radius_; + cell.avg_height_ = prev_cell.avg_height_; + cell.max_height_ = prev_cell.max_height_; + cell.min_height_ = prev_cell.min_height_; + cell.has_ground_ = false; + } + + cell.is_processed_ = true; + } + } +} + +// process the point cloud to segment the ground points +void GridGroundFilter::process( + const PointCloud2ConstPtr & in_cloud, pcl::PointIndices & out_no_ground_indices) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + // set input cloud + in_cloud_ = in_cloud; + + // clear the output indices + out_no_ground_indices.indices.clear(); + + // reset grid cells + grid_ptr_->resetCells(); + + // 1. assign points to grid cells + convert(); + + // 2. cell preprocess + preprocess(); + + // 3. initialize ground + initializeGround(out_no_ground_indices); + + // 4. classify point cloud + classify(out_no_ground_indices); +} + +} // namespace autoware::ground_segmentation diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp new file mode 100644 index 0000000000000..0024582f22b0a --- /dev/null +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp @@ -0,0 +1,221 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCAN_GROUND_FILTER__GRID_GROUND_FILTER_HPP_ +#define SCAN_GROUND_FILTER__GRID_GROUND_FILTER_HPP_ + +#include "data.hpp" +#include "grid.hpp" + +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::ground_segmentation +{ +using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; + +struct PointsCentroid +{ + float radius_avg; + float height_avg; + float height_max; + float height_min; + uint16_t grid_id; + std::vector pcl_indices; + std::vector height_list; + std::vector radius_list; + std::vector is_ground_list; + + PointsCentroid() + : radius_avg(0.0f), height_avg(0.0f), height_max(-10.0f), height_min(10.0f), grid_id(0) + { + } + + void initialize() + { + radius_avg = 0.0f; + height_avg = 0.0f; + height_max = -10.0f; + height_min = 10.0f; + grid_id = 0; + pcl_indices.clear(); + height_list.clear(); + radius_list.clear(); + is_ground_list.clear(); + } + + inline void addPoint(const float radius, const float height, const size_t index) + { + pcl_indices.push_back(index); + height_list.push_back(height); + radius_list.push_back(radius); + is_ground_list.push_back(true); + } + + int getGroundPointNum() const + { + return std::count(is_ground_list.begin(), is_ground_list.end(), true); + } + + void processAverage() + { + // process only if is_ground_list is true + const int ground_point_num = getGroundPointNum(); + if (ground_point_num == 0) { + return; + } + + float radius_sum = 0.0f; + float height_sum = 0.0f; + height_max = -10.0f; + height_min = 10.0f; + + for (size_t i = 0; i < is_ground_list.size(); ++i) { + if (!is_ground_list[i]) { + continue; + } + radius_sum += radius_list[i]; + height_sum += height_list[i]; + height_max = std::max(height_max, height_list[i]); + height_min = std::min(height_min, height_list[i]); + } + + radius_avg = radius_sum / ground_point_num; + height_avg = height_sum / ground_point_num; + } + + float getMinHeightOnly() const + { + float min_height = 10.0f; + for (size_t i = 0; i < is_ground_list.size(); ++i) { + if (!is_ground_list[i]) { + continue; + } + min_height = std::min(min_height, height_list[i]); + } + return min_height; + } + + float getAverageSlope() const { return std::atan2(height_avg, radius_avg); } + float getAverageHeight() const { return height_avg; } + float getAverageRadius() const { return radius_avg; } + float getMaxHeight() const { return height_max; } + float getMinHeight() const { return height_min; } + const std::vector & getIndicesRef() const { return pcl_indices; } + const std::vector & getHeightListRef() const { return height_list; } +}; + +struct GridGroundFilterParameter +{ + // parameters + float global_slope_max_angle_rad; + float local_slope_max_angle_rad; + float global_slope_max_ratio; + float local_slope_max_ratio; + float radial_divider_angle_rad; + size_t radial_dividers_num; + + bool use_recheck_ground_cluster; + bool use_lowest_point; + float detection_range_z_max; + float non_ground_height_threshold; + const uint16_t gnd_grid_continual_thresh = 3; + + float grid_size_m; + float grid_mode_switch_radius; + int gnd_grid_buffer_size; + float virtual_lidar_x; + float virtual_lidar_y; + float virtual_lidar_z; +}; + +class GridGroundFilter +{ +public: + explicit GridGroundFilter(GridGroundFilterParameter & param) : param_(param) + { + // calculate derived parameters + param_.global_slope_max_ratio = std::tan(param_.global_slope_max_angle_rad); + param_.local_slope_max_ratio = std::tan(param_.local_slope_max_angle_rad); + param_.radial_dividers_num = std::ceil(2.0 * M_PI / param_.radial_divider_angle_rad); + + // initialize grid pointer + grid_ptr_ = std::make_unique( + param_.virtual_lidar_x, param_.virtual_lidar_y, param_.virtual_lidar_z); + grid_ptr_->initialize( + param_.grid_size_m, param_.radial_divider_angle_rad, param_.grid_mode_switch_radius); + } + ~GridGroundFilter() = default; + + void setTimeKeeper(std::shared_ptr time_keeper_ptr) + { + time_keeper_ = std::move(time_keeper_ptr); + + // set time keeper for grid + grid_ptr_->setTimeKeeper(time_keeper_); + } + + void setDataAccessor(const PointCloud2ConstPtr & in_cloud) + { + if (!data_accessor_.isInitialized()) { + data_accessor_.setField(in_cloud); + } + } + void process(const PointCloud2ConstPtr & in_cloud, pcl::PointIndices & out_no_ground_indices); + +private: + // parameters + GridGroundFilterParameter param_; + + // data + PointCloud2ConstPtr in_cloud_; + PclDataAccessor data_accessor_; + + // grid data + std::unique_ptr grid_ptr_; + + // debug information + std::shared_ptr time_keeper_; + + bool recursiveSearch(const int check_idx, const int search_cnt, std::vector & idx) const; + bool recursiveSearch( + const int check_idx, const int search_cnt, std::vector & idx, size_t count) const; + void fitLineFromGndGrid(const std::vector & idx, float & a, float & b) const; + + void convert(); + void preprocess(); + void initializeGround(pcl::PointIndices & out_no_ground_indices); + + void SegmentContinuousCell( + const Cell & cell, PointsCentroid & ground_bin, pcl::PointIndices & out_no_ground_indices); + void SegmentDiscontinuousCell( + const Cell & cell, PointsCentroid & ground_bin, pcl::PointIndices & out_no_ground_indices); + void SegmentBreakCell( + const Cell & cell, PointsCentroid & ground_bin, pcl::PointIndices & out_no_ground_indices); + void classify(pcl::PointIndices & out_no_ground_indices); +}; + +} // namespace autoware::ground_segmentation + +#endif // SCAN_GROUND_FILTER__GRID_GROUND_FILTER_HPP_ diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp index 0d7c41802cca9..488de2da47f91 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -14,6 +14,8 @@ #include "node.hpp" +#include "grid_ground_filter.hpp" + #include #include #include @@ -55,8 +57,6 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); split_points_distance_tolerance_ = static_cast(declare_parameter("split_points_distance_tolerance")); - split_points_distance_tolerance_square_ = - split_points_distance_tolerance_ * split_points_distance_tolerance_; // vehicle info vehicle_info_ = VehicleInfoUtils(*this).getVehicleInfo(); @@ -81,11 +81,27 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); virtual_lidar_z_ = vehicle_info_.vehicle_height_m; - // initialize grid - grid_.initialize(grid_size_m_, grid_mode_switch_radius_, virtual_lidar_z_); - - // data access - data_offset_initialized_ = false; + // initialize grid filter + { + GridGroundFilterParameter param; + param.global_slope_max_angle_rad = global_slope_max_angle_rad_; + param.local_slope_max_angle_rad = local_slope_max_angle_rad_; + param.radial_divider_angle_rad = radial_divider_angle_rad_; + + param.use_recheck_ground_cluster = use_recheck_ground_cluster_; + param.use_lowest_point = use_lowest_point_; + param.detection_range_z_max = detection_range_z_max_; + param.non_ground_height_threshold = non_ground_height_threshold_; + + param.grid_size_m = grid_size_m_; + param.grid_mode_switch_radius = grid_mode_switch_radius_; + param.gnd_grid_buffer_size = gnd_grid_buffer_size_; + param.virtual_lidar_x = vehicle_info_.wheel_base_m / 2.0f + center_pcl_shift_; + param.virtual_lidar_y = 0.0f; + param.virtual_lidar_z = virtual_lidar_z_; + + grid_ground_filter_ptr_ = std::make_unique(param); + } } using std::placeholders::_1; @@ -108,86 +124,9 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & "~/debug/processing_time_detail_ms", 1); auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); time_keeper_ = std::make_shared(time_keeper); - } - } -} -inline void ScanGroundFilterComponent::set_field_index_offsets(const PointCloud2ConstPtr & input) -{ - data_offset_x_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; - data_offset_y_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; - data_offset_z_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; - int intensity_index = pcl::getFieldIndex(*input, "intensity"); - if (intensity_index != -1) { - data_offset_intensity_ = input->fields[intensity_index].offset; - intensity_type_ = input->fields[intensity_index].datatype; - } else { - data_offset_intensity_ = -1; - } - data_offset_initialized_ = true; -} - -inline void ScanGroundFilterComponent::get_point_from_data_index( - const PointCloud2ConstPtr & input, const size_t data_index, pcl::PointXYZ & point) const -{ - point.x = *reinterpret_cast(&input->data[data_index + data_offset_x_]); - point.y = *reinterpret_cast(&input->data[data_index + data_offset_y_]); - point.z = *reinterpret_cast(&input->data[data_index + data_offset_z_]); -} - -void ScanGroundFilterComponent::convertPointcloudGridScan( - const PointCloud2ConstPtr & in_cloud, - std::vector & out_radial_ordered_points) const -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - out_radial_ordered_points.resize(radial_dividers_num_); - PointData current_point; - - const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; - const auto x_shift = vehicle_info_.wheel_base_m / 2.0f + center_pcl_shift_; - - const size_t in_cloud_data_size = in_cloud->data.size(); - const size_t in_cloud_point_step = in_cloud->point_step; - - { // grouping pointcloud by its azimuth angle - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("azimuth_angle_grouping", *time_keeper_); - - size_t point_index = 0; - pcl::PointXYZ input_point; - for (size_t data_index = 0; data_index + in_cloud_point_step <= in_cloud_data_size; - data_index += in_cloud_point_step) { - // Get Point - get_point_from_data_index(in_cloud, data_index, input_point); - - // determine the azimuth angle group - auto x{input_point.x - x_shift}; // base on front wheel center - auto radius{static_cast(std::hypot(x, input_point.y))}; - auto theta{normalizeRadian(std::atan2(x, input_point.y), 0.0)}; - auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; - - current_point.radius = radius; - current_point.point_state = PointLabel::INIT; - current_point.orig_index = point_index; - current_point.grid_id = grid_.getGridId(radius); - - // store the point in the corresponding radial division - out_radial_ordered_points[radial_div].emplace_back(current_point); - ++point_index; - } - } - - { // sorting pointcloud by distance, on each azimuth angle group - std::unique_ptr inner_st_ptr; - if (time_keeper_) inner_st_ptr = std::make_unique("sort", *time_keeper_); - - for (size_t i = 0; i < radial_dividers_num_; ++i) { - std::sort( - out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); + // set time keeper to grid + grid_ground_filter_ptr_->setTimeKeeper(time_keeper_); } } } @@ -212,12 +151,11 @@ void ScanGroundFilterComponent::convertPointcloud( if (time_keeper_) inner_st_ptr = std::make_unique("azimuth_angle_grouping", *time_keeper_); - size_t point_index = 0; pcl::PointXYZ input_point; for (size_t data_index = 0; data_index + in_cloud_point_step <= in_cloud_data_size; data_index += in_cloud_point_step) { // Get Point - get_point_from_data_index(in_cloud, data_index, input_point); + data_accessor_.getPoint(in_cloud, data_index, input_point); // determine the azimuth angle group auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; @@ -226,11 +164,10 @@ void ScanGroundFilterComponent::convertPointcloud( current_point.radius = radius; current_point.point_state = PointLabel::INIT; - current_point.orig_index = point_index; + current_point.data_index = data_index; // store the point in the corresponding radial division out_radial_ordered_points[radial_div].emplace_back(current_point); - ++point_index; } } @@ -253,332 +190,6 @@ void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) c point.z = 0; } -void ScanGroundFilterComponent::initializeFirstGndGrids( - const float h, const float r, const uint16_t id, std::vector & gnd_grids) const -{ - // initialize gnd_grids - gnd_grids.clear(); - gnd_grids.reserve(gnd_grid_buffer_size_); - - // Index of grids - // id is the first grid, will be filled by initial centroid_bin - // id - gnd_grid_buffer_size_ is the first grid of zero-zero position - // the intermediate grids are generated by linear interpolation - const uint16_t estimating_grid_num = gnd_grid_buffer_size_ + 1; - const uint16_t idx_estimate_from = id - estimating_grid_num; - const float gradient = h / r; - - GridCenter curr_gnd_grid; - for (uint16_t idx = 1; idx < estimating_grid_num; ++idx) { - float interpolation_ratio = static_cast(idx) / static_cast(estimating_grid_num); - const uint16_t ind_grid = idx_estimate_from + idx; - - const float interpolated_r = r * interpolation_ratio; - const float interpolated_z = gradient * interpolated_r; - - curr_gnd_grid.radius = interpolated_r; - curr_gnd_grid.avg_height = interpolated_z; - curr_gnd_grid.max_height = interpolated_z; - curr_gnd_grid.gradient = gradient; - curr_gnd_grid.intercept = 0.0f; - curr_gnd_grid.grid_id = ind_grid; - gnd_grids.push_back(curr_gnd_grid); - } -} - -void ScanGroundFilterComponent::fitLineFromGndGrid( - const std::vector & gnd_grids_list, const size_t start_idx, const size_t end_idx, - float & a, float & b) const -{ - // calculate local gradient by least square method - float sum_x = 0.0f; - float sum_y = 0.0f; - float sum_xy = 0.0f; - float sum_x2 = 0.0f; - for (auto it = gnd_grids_list.begin() + start_idx; it < gnd_grids_list.begin() + end_idx; ++it) { - sum_x += it->radius; - sum_y += it->avg_height; - sum_xy += it->radius * it->avg_height; - sum_x2 += it->radius * it->radius; - } - const float n = static_cast(end_idx - start_idx); - - // y = a * x + b - a = (n * sum_xy - sum_x * sum_y) / (n * sum_x2 - sum_x * sum_x); - b = (sum_y - a * sum_x) / n; -} - -void ScanGroundFilterComponent::checkContinuousGndGrid( - PointData & pd, const pcl::PointXYZ & point_curr, - const std::vector & gnd_grids_list) const -{ - // 1. check local slope - { - // reference gird is the last-1 - const auto reference_grid_it = gnd_grids_list.end() - 2; - const float delta_z = point_curr.z - reference_grid_it->avg_height; - const float delta_radius = pd.radius - reference_grid_it->radius; - const float local_slope_ratio = delta_z / delta_radius; - - if (abs(local_slope_ratio) < local_slope_max_ratio_) { - pd.point_state = PointLabel::GROUND; - return; - } - } - - // 2. mean of grid buffer(filtering) - const float gradient = - std::clamp(gnd_grids_list.back().gradient, -global_slope_max_ratio_, global_slope_max_ratio_); - const float & intercept = gnd_grids_list.back().intercept; - - // extrapolate next ground height - const float next_gnd_z = gradient * pd.radius + intercept; - - // calculate fixed angular threshold from the reference position - const float gnd_z_local_thresh = - std::tan(DEG2RAD(5.0)) * (pd.radius - gnd_grids_list.back().radius); - - if (abs(point_curr.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh) { - pd.point_state = PointLabel::GROUND; - return; - } - if (point_curr.z - next_gnd_z >= non_ground_height_threshold_ + gnd_z_local_thresh) { - pd.point_state = PointLabel::NON_GROUND; - return; - } -} - -void ScanGroundFilterComponent::checkDiscontinuousGndGrid( - PointData & pd, const pcl::PointXYZ & point_curr, - const std::vector & gnd_grids_list) const -{ - const auto & grid_ref = gnd_grids_list.back(); - const float delta_avg_z = point_curr.z - grid_ref.avg_height; - if (abs(delta_avg_z) < non_ground_height_threshold_) { - pd.point_state = PointLabel::GROUND; - return; - } - const float delta_max_z = point_curr.z - grid_ref.max_height; - if (abs(delta_max_z) < non_ground_height_threshold_) { - pd.point_state = PointLabel::GROUND; - return; - } - const float delta_radius = pd.radius - grid_ref.radius; - const float local_slope_ratio = delta_avg_z / delta_radius; - if (abs(local_slope_ratio) < local_slope_max_ratio_) { - pd.point_state = PointLabel::GROUND; - return; - } - if (local_slope_ratio >= local_slope_max_ratio_) { - pd.point_state = PointLabel::NON_GROUND; - return; - } -} - -void ScanGroundFilterComponent::checkBreakGndGrid( - PointData & pd, const pcl::PointXYZ & point_curr, - const std::vector & gnd_grids_list) const -{ - const auto & grid_ref = gnd_grids_list.back(); - const float delta_avg_z = point_curr.z - grid_ref.avg_height; - const float delta_radius = pd.radius - grid_ref.radius; - const float local_slope_ratio = delta_avg_z / delta_radius; - if (abs(local_slope_ratio) < global_slope_max_ratio_) { - pd.point_state = PointLabel::GROUND; - return; - } - if (local_slope_ratio >= global_slope_max_ratio_) { - pd.point_state = PointLabel::NON_GROUND; - return; - } -} - -void ScanGroundFilterComponent::recheckGroundCluster( - const PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, - pcl::PointIndices & non_ground_indices) const -{ - const float reference_height = - use_lowest_point ? gnd_cluster.getMinHeight() : gnd_cluster.getAverageHeight(); - const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef(); - const std::vector & height_list = gnd_cluster.getHeightListRef(); - for (size_t i = 0; i < height_list.size(); ++i) { - if (height_list.at(i) >= reference_height + non_ground_threshold) { - non_ground_indices.indices.push_back(gnd_indices.indices.at(i)); - } - } -} - -void ScanGroundFilterComponent::classifyPointCloudGridScan( - const PointCloud2ConstPtr & in_cloud, - const std::vector & in_radial_ordered_clouds, - pcl::PointIndices & out_no_ground_indices) const -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - out_no_ground_indices.indices.clear(); - - // run the classification algorithm for each ray (azimuth division) - for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) { - PointsCentroid centroid_bin; - centroid_bin.initialize(); - std::vector gnd_grids; - - // check empty ray - if (in_radial_ordered_clouds[i].size() == 0) { - continue; - } - - bool initialized_first_gnd_grid = false; - - PointData pd_curr, pd_prev; - pcl::PointXYZ point_curr, point_prev; - - // initialize the previous point - { - pd_curr = in_radial_ordered_clouds[i][0]; - const size_t data_index = in_cloud->point_step * pd_curr.orig_index; - get_point_from_data_index(in_cloud, data_index, point_curr); - } - - // iterate over the points in the ray - for (const auto & point : in_radial_ordered_clouds[i]) { - // set the previous point - pd_prev = pd_curr; - point_prev = point_curr; - - // set the current point - pd_curr = point; - const size_t data_index = in_cloud->point_step * pd_curr.orig_index; - get_point_from_data_index(in_cloud, data_index, point_curr); - - // determine if the current point is in new grid - const bool is_curr_in_next_grid = pd_curr.grid_id > pd_prev.grid_id; - - // initialization process for the first grid and interpolate the previous grids - if (!initialized_first_gnd_grid) { - // set the thresholds - const float global_slope_ratio_p = point_prev.z / pd_prev.radius; - float non_ground_height_threshold_local = non_ground_height_threshold_; - if (point_prev.x < low_priority_region_x_) { - non_ground_height_threshold_local = - non_ground_height_threshold_ * abs(point_prev.x / low_priority_region_x_); - } - // non_ground_height_threshold_local is only for initialization - - // prepare centroid_bin for the first grid - if ( - // classify previous point - global_slope_ratio_p >= global_slope_max_ratio_ && - point_prev.z > non_ground_height_threshold_local) { - out_no_ground_indices.indices.push_back(pd_prev.orig_index); - pd_prev.point_state = PointLabel::NON_GROUND; - } else if ( - abs(global_slope_ratio_p) < global_slope_max_ratio_ && - abs(point_prev.z) < non_ground_height_threshold_local) { - centroid_bin.addPoint(pd_prev.radius, point_prev.z, pd_prev.orig_index); - pd_prev.point_state = PointLabel::GROUND; - // centroid_bin is filled at least once - // if the current point is in the next gird, it is ready to be initialized - initialized_first_gnd_grid = is_curr_in_next_grid; - } - // keep filling the centroid_bin until it is ready to be initialized - if (!initialized_first_gnd_grid) { - continue; - } - // estimate previous grids by linear interpolation - float h = centroid_bin.getAverageHeight(); - float r = centroid_bin.getAverageRadius(); - initializeFirstGndGrids(h, r, pd_prev.grid_id, gnd_grids); - } - - // finalize the current centroid_bin and update the gnd_grids - if (is_curr_in_next_grid && centroid_bin.getIndicesRef().indices.size() > 0) { - // check if the prev grid have ground point cloud - if (use_recheck_ground_cluster_) { - recheckGroundCluster( - centroid_bin, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices); - // centroid_bin is not modified. should be rechecked by out_no_ground_indices? - } - // convert the centroid_bin to grid-center and add it to the gnd_grids - GridCenter curr_gnd_grid; - curr_gnd_grid.radius = centroid_bin.getAverageRadius(); - curr_gnd_grid.avg_height = centroid_bin.getAverageHeight(); - curr_gnd_grid.max_height = centroid_bin.getMaxHeight(); - curr_gnd_grid.grid_id = pd_prev.grid_id; - curr_gnd_grid.gradient = 0.0f; // not calculated yet - curr_gnd_grid.intercept = 0.0f; // not calculated yet - gnd_grids.push_back(curr_gnd_grid); - // clear the centroid_bin - centroid_bin.initialize(); - - // calculate local ground gradient - float gradient, intercept; - fitLineFromGndGrid( - gnd_grids, gnd_grids.size() - gnd_grid_buffer_size_, gnd_grids.size(), gradient, - intercept); - // update the current grid - gnd_grids.back().gradient = gradient; // update the gradient - gnd_grids.back().intercept = intercept; // update the intercept - } - - // 0: set the thresholds - const float global_slope_ratio_p = point_curr.z / pd_curr.radius; - const auto & grid_ref = gnd_grids.back(); - - // 1: height is out-of-range - if (point_curr.z - grid_ref.avg_height > detection_range_z_max_) { - pd_curr.point_state = PointLabel::OUT_OF_RANGE; - continue; - } - - // 2: continuously non-ground - float points_xy_distance_square = - (point_curr.x - point_prev.x) * (point_curr.x - point_prev.x) + - (point_curr.y - point_prev.y) * (point_curr.y - point_prev.y); - if ( - pd_prev.point_state == PointLabel::NON_GROUND && - points_xy_distance_square < split_points_distance_tolerance_square_ && - point_curr.z > point_prev.z) { - pd_curr.point_state = PointLabel::NON_GROUND; - out_no_ground_indices.indices.push_back(pd_curr.orig_index); - continue; - } - - // 3: the angle is exceed the global slope threshold - if (global_slope_ratio_p > global_slope_max_ratio_) { - out_no_ground_indices.indices.push_back(pd_curr.orig_index); - continue; - } - - const uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id + - gnd_grid_buffer_size_ + gnd_grid_continual_thresh_; - const float curr_grid_width = grid_.getGridSize(pd_curr.radius, pd_curr.grid_id); - if ( - // 4: the point is continuous with the previous grid - pd_curr.grid_id < next_gnd_grid_id_thresh && - pd_curr.radius - grid_ref.radius < gnd_grid_continual_thresh_ * curr_grid_width) { - checkContinuousGndGrid(pd_curr, point_curr, gnd_grids); - } else if ( - // 5: the point is discontinuous with the previous grid - pd_curr.radius - grid_ref.radius < gnd_grid_continual_thresh_ * curr_grid_width) { - checkDiscontinuousGndGrid(pd_curr, point_curr, gnd_grids); - } else { - // 6: the point is break the previous grid - checkBreakGndGrid(pd_curr, point_curr, gnd_grids); - } - - // update the point label and update the ground cluster - if (pd_curr.point_state == PointLabel::NON_GROUND) { - out_no_ground_indices.indices.push_back(pd_curr.orig_index); - } else if (pd_curr.point_state == PointLabel::GROUND) { - centroid_bin.addPoint(pd_curr.radius, point_curr.z, pd_curr.orig_index); - } - // else, the point is not classified - } - } -} - void ScanGroundFilterComponent::classifyPointCloud( const PointCloud2ConstPtr & in_cloud, const std::vector & in_radial_ordered_clouds, @@ -615,8 +226,7 @@ void ScanGroundFilterComponent::classifyPointCloud( const PointData & pd = in_radial_ordered_clouds[i][j]; point_label_curr = pd.point_state; - const size_t data_index = in_cloud->point_step * pd.orig_index; - get_point_from_data_index(in_cloud, data_index, point_curr); + data_accessor_.getPoint(in_cloud, pd.data_index, point_curr); if (j == 0) { bool is_front_side = (point_curr.x > virtual_ground_point.x); if (use_virtual_ground_point_ && is_front_side) { @@ -677,12 +287,12 @@ void ScanGroundFilterComponent::classifyPointCloud( non_ground_cluster.initialize(); } if (point_label_curr == PointLabel::NON_GROUND) { - out_no_ground_indices.indices.push_back(pd.orig_index); + out_no_ground_indices.indices.push_back(pd.data_index); } else if ( // NOLINT (point_label_prev == PointLabel::NON_GROUND) && (point_label_curr == PointLabel::POINT_FOLLOW)) { point_label_curr = PointLabel::NON_GROUND; - out_no_ground_indices.indices.push_back(pd.orig_index); + out_no_ground_indices.indices.push_back(pd.data_index); } else if ( // NOLINT (point_label_prev == PointLabel::GROUND) && (point_label_curr == PointLabel::POINT_FOLLOW)) { @@ -714,9 +324,9 @@ void ScanGroundFilterComponent::extractObjectPoints( size_t output_data_size = 0; - for (const auto & i : in_indices.indices) { + for (const auto & idx : in_indices.indices) { std::memcpy( - &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step], + &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[idx], in_cloud_ptr->point_step * sizeof(uint8_t)); output_data_size += in_cloud_ptr->point_step; } @@ -731,19 +341,19 @@ void ScanGroundFilterComponent::faster_filter( if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); std::scoped_lock lock(mutex_); - stop_watch_ptr_->toc("processing_time", true); + if (stop_watch_ptr_) stop_watch_ptr_->toc("processing_time", true); - if (!data_offset_initialized_) { - set_field_index_offsets(input); + if (!data_accessor_.isInitialized()) { + data_accessor_.setField(input); + grid_ground_filter_ptr_->setDataAccessor(input); } - std::vector radial_ordered_points; pcl::PointIndices no_ground_indices; if (elevation_grid_mode_) { - convertPointcloudGridScan(input, radial_ordered_points); - classifyPointCloudGridScan(input, radial_ordered_points, no_ground_indices); + grid_ground_filter_ptr_->process(input, no_ground_indices); } else { + std::vector radial_ordered_points; convertPointcloud(input, radial_ordered_points); classifyPointCloud(input, radial_ordered_points, no_ground_indices); } @@ -783,10 +393,10 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( const std::vector & param) { if (get_param(param, "grid_size_m", grid_size_m_)) { - grid_.initialize(grid_size_m_, grid_mode_switch_radius_, virtual_lidar_z_); + // grid_ptr_->initialize(grid_size_m_, radial_divider_angle_rad_, grid_mode_switch_radius_); } if (get_param(param, "grid_mode_switch_radius", grid_mode_switch_radius_)) { - grid_.initialize(grid_size_m_, grid_mode_switch_radius_, virtual_lidar_z_); + // grid_ptr_->initialize(grid_size_m_, radial_divider_angle_rad_, grid_mode_switch_radius_); } double global_slope_max_angle_deg{get_parameter("global_slope_max_angle_deg").as_double()}; if (get_param(param, "global_slope_max_angle_deg", global_slope_max_angle_deg)) { @@ -808,19 +418,15 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( if (get_param(param, "radial_divider_angle_deg", radial_divider_angle_deg)) { radial_divider_angle_rad_ = deg2rad(radial_divider_angle_deg); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); + // grid_ptr_->initialize(grid_size_m_, radial_divider_angle_rad_, grid_mode_switch_radius_); RCLCPP_DEBUG( get_logger(), "Setting radial_divider_angle_rad to: %f.", radial_divider_angle_rad_); RCLCPP_DEBUG(get_logger(), "Setting radial_dividers_num to: %zu.", radial_dividers_num_); } if (get_param(param, "split_points_distance_tolerance", split_points_distance_tolerance_)) { - split_points_distance_tolerance_square_ = - split_points_distance_tolerance_ * split_points_distance_tolerance_; RCLCPP_DEBUG( get_logger(), "Setting split_points_distance_tolerance to: %f.", split_points_distance_tolerance_); - RCLCPP_DEBUG( - get_logger(), "Setting split_points_distance_tolerance_square to: %f.", - split_points_distance_tolerance_square_); } if (get_param(param, "split_height_distance", split_height_distance_)) { RCLCPP_DEBUG(get_logger(), "Setting split_height_distance to: %f.", split_height_distance_); diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp index acc6114ed0acc..41c41f1899163 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp @@ -15,11 +15,13 @@ #ifndef SCAN_GROUND_FILTER__NODE_HPP_ #define SCAN_GROUND_FILTER__NODE_HPP_ -#include "autoware/pointcloud_preprocessor/filter.hpp" -#include "autoware/pointcloud_preprocessor/transform_info.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" -#include "autoware_vehicle_info_utils/vehicle_info.hpp" -#include "grid.hpp" +#include "data.hpp" +#include "grid_ground_filter.hpp" + +#include +#include +#include +#include #include @@ -36,6 +38,7 @@ #include +#include #include #include #include @@ -67,20 +70,10 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt float radius; // cylindrical coords on XY Plane PointLabel point_state{PointLabel::INIT}; uint16_t grid_id; // id of grid in vertical - size_t orig_index; // index of this point in the source pointcloud + size_t data_index; // index of this point data in the source pointcloud }; using PointCloudVector = std::vector; - struct GridCenter - { - float radius; - float avg_height; - float max_height; - float gradient; - float intercept; - uint16_t grid_id; - }; - struct PointsCentroid { float radius_sum; @@ -91,8 +84,9 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt float height_min; uint32_t point_num; uint16_t grid_id; - pcl::PointIndices pcl_indices; + std::vector pcl_indices; std::vector height_list; + std::vector radius_list; PointsCentroid() : radius_sum(0.0f), @@ -116,7 +110,7 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt height_min = 10.0f; point_num = 0; grid_id = 0; - pcl_indices.indices.clear(); + pcl_indices.clear(); height_list.clear(); } @@ -130,24 +124,20 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt height_max = height_max < height ? height : height_max; height_min = height_min > height ? height : height_min; } - void addPoint(const float radius, const float height, const uint index) + + void addPoint(const float radius, const float height, const size_t index) { - pcl_indices.indices.push_back(index); + pcl_indices.push_back(index); height_list.push_back(height); addPoint(radius, height); } float getAverageSlope() const { return std::atan2(height_avg, radius_avg); } - float getAverageHeight() const { return height_avg; } - float getAverageRadius() const { return radius_avg; } - float getMaxHeight() const { return height_max; } - float getMinHeight() const { return height_min; } - - const pcl::PointIndices & getIndicesRef() const { return pcl_indices; } + const std::vector & getIndicesRef() const { return pcl_indices; } const std::vector & getHeightListRef() const { return height_list; } }; @@ -156,19 +146,15 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt // TODO(taisa1): Temporary Implementation: Remove this interface when all the filter nodes // conform to new API - virtual void faster_filter( + void faster_filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, - const autoware::pointcloud_preprocessor::TransformInfo & transform_info); + const autoware::pointcloud_preprocessor::TransformInfo & transform_info) override; tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; - int data_offset_x_; - int data_offset_y_; - int data_offset_z_; - int data_offset_intensity_; - int intensity_type_; - bool data_offset_initialized_; + // data accessor + PclDataAccessor data_accessor_; const uint16_t gnd_grid_continual_thresh_ = 3; bool elevation_grid_mode_; @@ -187,7 +173,6 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt float global_slope_max_ratio_; float local_slope_max_ratio_; float split_points_distance_tolerance_; // distance in meters between concentric divisions - float split_points_distance_tolerance_square_; // non-grid mode parameters bool use_virtual_ground_point_; @@ -206,13 +191,8 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt uint16_t gnd_grid_buffer_size_; float virtual_lidar_z_; - // grid data - ScanGroundGrid grid_; - - // data access methods - void set_field_index_offsets(const PointCloud2ConstPtr & input); - void get_point_from_data_index( - const PointCloud2ConstPtr & input, const size_t data_index, pcl::PointXYZ & point) const; + // grid ground filter processor + std::unique_ptr grid_ground_filter_ptr_; // time keeper related rclcpp::Publisher::SharedPtr @@ -237,9 +217,7 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt void convertPointcloud( const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) const; - void convertPointcloudGridScan( - const PointCloud2ConstPtr & in_cloud, - std::vector & out_radial_ordered_points) const; + /*! * Output ground center of front wheels as the virtual ground point * @param[out] point Virtual ground origin point @@ -253,39 +231,10 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt * @param out_no_ground_indices Returns the indices of the points * classified as not ground in the original PointCloud */ - - void initializeFirstGndGrids( - const float h, const float r, const uint16_t id, std::vector & gnd_grids) const; - - void fitLineFromGndGrid( - const std::vector & gnd_grids_list, const size_t start_idx, const size_t end_idx, - float & a, float & b) const; - void checkContinuousGndGrid( - PointData & pd, const pcl::PointXYZ & point_curr, - const std::vector & gnd_grids_list) const; - void checkDiscontinuousGndGrid( - PointData & pd, const pcl::PointXYZ & point_curr, - const std::vector & gnd_grids_list) const; - void checkBreakGndGrid( - PointData & pd, const pcl::PointXYZ & point_curr, - const std::vector & gnd_grids_list) const; void classifyPointCloud( const PointCloud2ConstPtr & in_cloud, const std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) const; - void classifyPointCloudGridScan( - const PointCloud2ConstPtr & in_cloud, - const std::vector & in_radial_ordered_clouds, - pcl::PointIndices & out_no_ground_indices) const; - /*! - * Re-classifies point of ground cluster based on their height - * @param gnd_cluster Input ground cluster for re-checking - * @param non_ground_threshold Height threshold for ground and non-ground points classification - * @param non_ground_indices Output non-ground PointCloud indices - */ - void recheckGroundCluster( - const PointsCentroid & gnd_cluster, const float non_ground_threshold, - const bool use_lowest_point, pcl::PointIndices & non_ground_indices) const; /*! * Returns the resulting complementary PointCloud, one with the points kept * and the other removed as indicated in the indices diff --git a/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp b/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp index a3f8b9d4b19b3..5e4f16e359607 100644 --- a/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp +++ b/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp @@ -24,6 +24,11 @@ #include +#include +#include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #include diff --git a/perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp b/perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp index 1e494f683f181..3626aaa837309 100644 --- a/perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp +++ b/perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp @@ -21,6 +21,9 @@ #include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #include diff --git a/perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp b/perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp index 706ffff3ce511..2c0f649168703 100644 --- a/perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp @@ -20,6 +20,9 @@ #include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #include diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/debugger.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/debugger.hpp index ba7f8b6ca3496..f4229382ba38c 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/debugger.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/debugger.hpp @@ -56,7 +56,6 @@ class Debugger void imageCallback( const sensor_msgs::msg::Image::ConstSharedPtr input_image_msg, const std::size_t image_id); - rclcpp::Node * node_ptr_; std::shared_ptr image_transport_; std::vector input_camera_topics_; std::vector image_subs_; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index aa3e3fe096306..8c0e2ed78fc6c 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -76,7 +76,7 @@ class PointPaintingFusionNode std::unique_ptr detector_ptr_{nullptr}; - bool out_of_scope(const DetectedObjects & obj); + bool out_of_scope(const DetectedObjects & obj) override; }; } // namespace autoware::image_projection_based_fusion #endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp index abececee35c25..2c0283a190023 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -54,7 +54,7 @@ class RoiClusterFusionNode double trust_object_distance_; std::string non_trust_object_iou_mode_{"iou_x"}; bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); - bool out_of_scope(const DetectedObjectWithFeature & obj); + bool out_of_scope(const DetectedObjectWithFeature & obj) override; double cal_iou_by_mode( const sensor_msgs::msg::RegionOfInterest & roi_1, const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index ea15a2df5efe2..7103537ec852d 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -57,7 +57,7 @@ class RoiDetectedObjectFusionNode void publish(const DetectedObjects & output_msg) override; - bool out_of_scope(const DetectedObject & obj); + bool out_of_scope(const DetectedObject & obj) override; private: struct diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index c7cad2b5a64d0..4de49df61a071 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -49,7 +49,7 @@ class RoiPointCloudFusionNode const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; - bool out_of_scope(const DetectedObjectWithFeature & obj); + bool out_of_scope(const DetectedObjectWithFeature & obj) override; }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 152829ea769d3..b09e9521bdcdb 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -62,7 +62,7 @@ class SegmentPointCloudFusionNode : public FusionNode +#include +#include + #if __has_include() #include #else @@ -41,7 +45,7 @@ namespace autoware::image_projection_based_fusion Debugger::Debugger( rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size, std::vector input_camera_topics) -: node_ptr_(node_ptr), input_camera_topics_{input_camera_topics} +: input_camera_topics_{input_camera_topics} { image_buffers_.resize(image_num); image_buffer_size_ = image_buffer_size; diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 16a089fcc6d09..60406652310dd 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -24,6 +24,9 @@ #include #include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -212,7 +215,7 @@ void FusionNode::subCallback( preprocess(*output_msg); int64_t timestamp_nsec = - (*output_msg).header.stamp.sec * (int64_t)1e9 + (*output_msg).header.stamp.nanosec; + (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; // if matching rois exist, fuseOnSingle // please ask maintainers before parallelize this loop because debugger is not thread safe @@ -229,14 +232,17 @@ void FusionNode::subCallback( std::list outdate_stamps; for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { - int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * (int64_t)1e6; - int64_t interval = abs(int64_t(k) - new_stamp); + int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); + int64_t interval = abs(static_cast(k) - new_stamp); - if (interval <= min_interval && interval <= match_threshold_ms_ * (int64_t)1e6) { + if ( + interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { min_interval = interval; matched_stamp = k; - } else if (int64_t(k) < new_stamp && interval > match_threshold_ms_ * (int64_t)1e6) { - outdate_stamps.push_back(int64_t(k)); + } else if ( + static_cast(k) < new_stamp && + interval > match_threshold_ms_ * static_cast(1e6)) { + outdate_stamps.push_back(static_cast(k)); } } @@ -290,7 +296,7 @@ void FusionNode::subCallback( processing_time_ms = 0; } } else { - cached_msg_.first = int64_t(timestamp_nsec); + cached_msg_.first = timestamp_nsec; cached_msg_.second = output_msg; processing_time_ms = stop_watch_ptr_->toc("processing_time", true); } @@ -302,15 +308,16 @@ void FusionNode::roiCallback( { stop_watch_ptr_->toc("processing_time", true); - int64_t timestamp_nsec = - (*input_roi_msg).header.stamp.sec * (int64_t)1e9 + (*input_roi_msg).header.stamp.nanosec; + int64_t timestamp_nsec = (*input_roi_msg).header.stamp.sec * static_cast(1e9) + + (*input_roi_msg).header.stamp.nanosec; // if cached Msg exist, try to match if (cached_msg_.second != nullptr) { - int64_t new_stamp = cached_msg_.first + input_offset_ms_.at(roi_i) * (int64_t)1e6; + int64_t new_stamp = cached_msg_.first + input_offset_ms_.at(roi_i) * static_cast(1e6); int64_t interval = abs(timestamp_nsec - new_stamp); - if (interval < match_threshold_ms_ * (int64_t)1e6 && is_fused_.at(roi_i) == false) { + if ( + interval < match_threshold_ms_ * static_cast(1e6) && is_fused_.at(roi_i) == false) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index ac9ed88c2d98c..4e0f88e2134ac 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -29,6 +29,9 @@ #include #include +#include +#include +#include namespace { diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index 205d231ab32bd..e3813a020e319 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -16,6 +16,8 @@ #include +#include + namespace autoware::image_projection_based_fusion { diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index e925efdc0afcb..9a94613a2b788 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -20,6 +20,12 @@ #include #include +#include +#include +#include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #include diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 3a67d37c415c8..134dd7bad9129 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -19,6 +19,11 @@ #include #include +#include +#include +#include +#include + namespace autoware::image_projection_based_fusion { @@ -64,7 +69,7 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) } int64_t timestamp_nsec = - output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec; + output_msg.header.stamp.sec * static_cast(1e9) + output_msg.header.stamp.nanosec; passthrough_object_flags_map_.insert(std::make_pair(timestamp_nsec, passthrough_object_flags)); fused_object_flags_map_.insert(std::make_pair(timestamp_nsec, fused_object_flags)); ignored_object_flags_map_.insert(std::make_pair(timestamp_nsec, ignored_object_flags)); @@ -113,8 +118,8 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( const image_geometry::PinholeCameraModel & pinhole_camera_model) { std::map object_roi_map; - int64_t timestamp_nsec = - input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + int64_t timestamp_nsec = input_object_msg.header.stamp.sec * static_cast(1e9) + + input_object_msg.header.stamp.nanosec; if (passthrough_object_flags_map_.size() == 0) { return object_roi_map; } @@ -200,8 +205,8 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( const std::vector & image_rois, const std::map & object_roi_map) { - int64_t timestamp_nsec = - input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + int64_t timestamp_nsec = input_object_msg.header.stamp.sec * static_cast(1e9) + + input_object_msg.header.stamp.nanosec; if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { return; } @@ -284,7 +289,7 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) } int64_t timestamp_nsec = - output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec; + output_msg.header.stamp.sec * static_cast(1e9) + output_msg.header.stamp.nanosec; if ( passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 039e4f7591bbc..f9eb4ef909282 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -17,6 +17,8 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include + #ifdef ROS_DISTRO_GALACTIC #include #include @@ -85,6 +87,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (!checkCameraInfo(camera_info)) return; std::vector output_objs; + std::vector debug_image_rois; + std::vector debug_image_points; // select ROIs for fusion for (const auto & feature_obj : input_roi_msg.feature_objects) { if (fuse_unknown_only_) { @@ -92,10 +96,12 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; if (is_roi_label_unknown) { output_objs.push_back(feature_obj); + debug_image_rois.push_back(feature_obj.feature.roi); } } else { // TODO(badai-nguyen): selected class from a list output_objs.push_back(feature_obj); + debug_image_rois.push_back(feature_obj.feature.roi); } } if (output_objs.empty()) { @@ -166,12 +172,25 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( clusters_data_size.at(i) += point_step; } } + if (debugger_) { + // add all points inside image to debug + if ( + projected_point.x() > 0 && projected_point.x() < camera_info.width && + projected_point.y() > 0 && projected_point.y() < camera_info.height) { + debug_image_points.push_back(projected_point); + } + } } // refine and update output_fused_objects_ updateOutputFusedObjects( output_objs, clusters, clusters_data_size, input_pointcloud_msg, input_roi_msg.header, tf_buffer_, min_cluster_size_, max_cluster_size_, cluster_2d_tolerance_, output_fused_objects_); + if (debugger_) { + debugger_->image_rois_ = debug_image_rois; + debugger_->obstacle_points_ = debug_image_points; + debugger_->publishImage(image_id, input_roi_msg.header.stamp); + } } bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 430b452b391f6..e678a956bc64e 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -19,6 +19,9 @@ #include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #include diff --git a/perception/autoware_image_projection_based_fusion/src/utils/geometry.cpp b/perception/autoware_image_projection_based_fusion/src/utils/geometry.cpp index 4bea6ac0fe713..a5a86cc0eb3a6 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/geometry.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/geometry.cpp @@ -16,6 +16,8 @@ #include +#include + namespace autoware::image_projection_based_fusion { diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 912f6e99355e4..338a5605e5a79 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -16,6 +16,10 @@ #include +#include +#include +#include + namespace autoware::image_projection_based_fusion { bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info) diff --git a/perception/autoware_image_projection_based_fusion/test/test_geometry.cpp b/perception/autoware_image_projection_based_fusion/test/test_geometry.cpp index cf3778e7a0c84..b51ad06eb7b88 100644 --- a/perception/autoware_image_projection_based_fusion/test/test_geometry.cpp +++ b/perception/autoware_image_projection_based_fusion/test/test_geometry.cpp @@ -16,6 +16,8 @@ #include +#include + TEST(objectToVertices, test_objectToVertices) { // Test `boundingBoxToVertices()` and `cylinderToVertices()` simultaneously diff --git a/perception/autoware_image_projection_based_fusion/test/test_utils.cpp b/perception/autoware_image_projection_based_fusion/test/test_utils.cpp index ac179c66dd3d0..79dd775928f41 100644 --- a/perception/autoware_image_projection_based_fusion/test/test_utils.cpp +++ b/perception/autoware_image_projection_based_fusion/test/test_utils.cpp @@ -17,6 +17,8 @@ #include +#include + using autoware::point_types::PointXYZI; void setPointCloud2Fields(sensor_msgs::msg::PointCloud2 & pointcloud) diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp index 86b1bf10c75a1..60aa09c8e16d7 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp @@ -22,8 +22,11 @@ #include #include +#include +#include #include #include +#include #include namespace autoware diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/feature_generator.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/feature_generator.cpp index e1aa0fc0494de..fa70863d9c800 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/feature_generator.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/feature_generator.cpp @@ -17,6 +17,7 @@ #include "autoware/lidar_apollo_instance_segmentation/log_table.hpp" #include +#include namespace { diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp index 3314b1c7421d3..9a3e13b81120f 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp @@ -19,6 +19,8 @@ #include #include +#include + namespace autoware { namespace lidar_apollo_instance_segmentation diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp index eb056ed14c57f..a49451fde9d0d 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp @@ -51,7 +51,6 @@ class LidarCenterPointNode : public rclcpp::Node rclcpp::Subscription::SharedPtr pointcloud_sub_; rclcpp::Publisher::SharedPtr objects_pub_; - float score_threshold_{0.0}; std::vector class_names_; bool has_variance_{false}; bool has_twist_{false}; diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp index 66384d192ae43..18d4f3abb3703 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp @@ -28,6 +28,7 @@ namespace autoware::lidar_centerpoint class VoxelGeneratorTemplate { public: + virtual ~VoxelGeneratorTemplate() = default; explicit VoxelGeneratorTemplate( const DensificationParam & param, const CenterPointConfig & config); diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 5f81b70ab6d15..6ee2b3733bdb0 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -135,8 +135,7 @@ bool CenterPointTRT::detect( cudaMemsetAsync(spatial_features_d_.get(), 0, spatial_features_size_ * sizeof(float), stream_)); if (!preprocess(input_pointcloud_msg, tf_buffer)) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect."); + RCLCPP_WARN(rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect."); return false; } @@ -144,6 +143,20 @@ bool CenterPointTRT::detect( postProcess(det_boxes3d); + // Check the actual number of pillars after inference to avoid unnecessary synchronization. + unsigned int num_pillars = 0; + CHECK_CUDA_ERROR( + cudaMemcpy(&num_pillars, num_voxels_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + if (num_pillars >= config_.max_voxel_size_) { + rclcpp::Clock clock{RCL_ROS_TIME}; + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("lidar_centerpoint"), clock, 1000, + "The actual number of pillars (%u) exceeds its maximum value (%zu). " + "Please considering increasing it since it may limit the detection performance.", + num_pillars, config_.max_voxel_size_); + } + return true; } diff --git a/perception/autoware_lidar_centerpoint/lib/detection_class_remapper.cpp b/perception/autoware_lidar_centerpoint/lib/detection_class_remapper.cpp index 80c842f8746a4..f2806bb84282b 100644 --- a/perception/autoware_lidar_centerpoint/lib/detection_class_remapper.cpp +++ b/perception/autoware_lidar_centerpoint/lib/detection_class_remapper.cpp @@ -14,6 +14,9 @@ #include "autoware/lidar_centerpoint/detection_class_remapper.hpp" +#include +#include + namespace autoware::lidar_centerpoint { diff --git a/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp b/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp index d0c0bb156f4a2..7767c0b5cbb02 100644 --- a/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp @@ -14,6 +14,9 @@ #include "autoware/lidar_centerpoint/network/network_trt.hpp" +#include +#include + namespace autoware::lidar_centerpoint { bool VoxelEncoderTRT::setProfile( diff --git a/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index 8f3f0fa9b20e2..9764d20628f24 100644 --- a/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -18,6 +18,8 @@ #include #include +#include + namespace autoware::lidar_centerpoint { diff --git a/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp index f975449530e5c..208c96a369182 100644 --- a/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -19,6 +19,7 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" +#include #include namespace autoware::lidar_centerpoint diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index c76fa6567ab51..0ea681538d8c1 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -18,6 +18,9 @@ #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/constants.hpp" +#include +#include + namespace autoware::lidar_centerpoint { diff --git a/perception/autoware_lidar_centerpoint/test/test_detection_class_remapper.cpp b/perception/autoware_lidar_centerpoint/test/test_detection_class_remapper.cpp index 48642bf7227dc..ae893c38b8d6a 100644 --- a/perception/autoware_lidar_centerpoint/test/test_detection_class_remapper.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_detection_class_remapper.cpp @@ -16,6 +16,8 @@ #include +#include + TEST(DetectionClassRemapperTest, MapClasses) { autoware::lidar_centerpoint::DetectionClassRemapper remapper; diff --git a/perception/autoware_lidar_centerpoint/test/test_nms.cpp b/perception/autoware_lidar_centerpoint/test/test_nms.cpp index 4a59d4dba98bf..fd9e78b0c6585 100644 --- a/perception/autoware_lidar_centerpoint/test/test_nms.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_nms.cpp @@ -16,6 +16,8 @@ #include +#include + TEST(NonMaximumSuppressionTest, Apply) { autoware::lidar_centerpoint::NonMaximumSuppression nms; diff --git a/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp index 5605d2df6a9d9..d45d18f51cc4e 100644 --- a/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp @@ -16,6 +16,9 @@ #include +#include +#include + TEST(TestSuite, box3DToDetectedObject) { std::vector class_names = {"CAR", "TRUCK", "BUS", "TRAILER", diff --git a/perception/autoware_lidar_centerpoint/test/test_voxel_generator.cpp b/perception/autoware_lidar_centerpoint/test/test_voxel_generator.cpp index a5b5337e3b24e..4b94cb983c862 100644 --- a/perception/autoware_lidar_centerpoint/test/test_voxel_generator.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_voxel_generator.cpp @@ -18,6 +18,9 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" +#include +#include + void VoxelGeneratorTest::SetUp() { // Setup things that should occur before every test instance should go here diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp index e19d2d49af998..466c2202de3e3 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp @@ -55,7 +55,6 @@ class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_{tf_buffer_}; DetectionClassRemapper detection_class_remapper_; - float score_threshold_{0.0}; std::vector class_names_; NonMaximumSuppression iou_bev_nms_; diff --git a/perception/autoware_lidar_transfusion/lib/detection_class_remapper.cpp b/perception/autoware_lidar_transfusion/lib/detection_class_remapper.cpp index 08bdc65e666de..661cf9b8cded4 100644 --- a/perception/autoware_lidar_transfusion/lib/detection_class_remapper.cpp +++ b/perception/autoware_lidar_transfusion/lib/detection_class_remapper.cpp @@ -14,6 +14,9 @@ #include +#include +#include + namespace autoware::lidar_transfusion { diff --git a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp index 052b7627166f9..8e3cb8a55ec7e 100644 --- a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include namespace autoware::lidar_transfusion { diff --git a/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp index 1ce151d2960ef..1a288d1b0fd44 100644 --- a/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -18,6 +18,8 @@ #include #include +#include + namespace autoware::lidar_transfusion { diff --git a/perception/autoware_lidar_transfusion/lib/preprocess/voxel_generator.cpp b/perception/autoware_lidar_transfusion/lib/preprocess/voxel_generator.cpp index 203a556d9057a..80ae1fae0eb1d 100644 --- a/perception/autoware_lidar_transfusion/lib/preprocess/voxel_generator.cpp +++ b/perception/autoware_lidar_transfusion/lib/preprocess/voxel_generator.cpp @@ -18,6 +18,9 @@ #include +#include +#include +#include #include namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/lib/ros_utils.cpp b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp index 88f6497f8d656..82f95697100cd 100644 --- a/perception/autoware_lidar_transfusion/lib/ros_utils.cpp +++ b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp @@ -18,6 +18,9 @@ #include #include +#include +#include + namespace autoware::lidar_transfusion { diff --git a/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp index 42e017a2dfdda..4e100f2e794d5 100644 --- a/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include namespace autoware::lidar_transfusion @@ -175,6 +176,13 @@ bool TransfusionTRT::preprocess( } if (params_input > config_.max_voxels_) { + rclcpp::Clock clock{RCL_ROS_TIME}; + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("lidar_transfusion"), clock, 1000, + "The actual number of voxels (%u) exceeds its maximum value (%zu). " + "Please considering increasing it since it may limit the detection performance.", + params_input, config_.max_voxels_); + params_input = config_.max_voxels_; } RCLCPP_DEBUG_STREAM( diff --git a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp index fae1cc97404da..c75a5f434aeed 100644 --- a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp @@ -16,6 +16,11 @@ #include "autoware/lidar_transfusion/utils.hpp" +#include +#include +#include +#include + namespace autoware::lidar_transfusion { diff --git a/perception/autoware_lidar_transfusion/test/test_detection_class_remapper.cpp b/perception/autoware_lidar_transfusion/test/test_detection_class_remapper.cpp index a9b5c8e83fbf7..ed79d362ba73d 100644 --- a/perception/autoware_lidar_transfusion/test/test_detection_class_remapper.cpp +++ b/perception/autoware_lidar_transfusion/test/test_detection_class_remapper.cpp @@ -16,6 +16,8 @@ #include +#include + TEST(DetectionClassRemapperTest, MapClasses) { autoware::lidar_transfusion::DetectionClassRemapper remapper; diff --git a/perception/autoware_lidar_transfusion/test/test_nms.cpp b/perception/autoware_lidar_transfusion/test/test_nms.cpp index 654cb56078c57..5449f61aa2284 100644 --- a/perception/autoware_lidar_transfusion/test/test_nms.cpp +++ b/perception/autoware_lidar_transfusion/test/test_nms.cpp @@ -16,6 +16,8 @@ #include +#include + TEST(NonMaximumSuppressionTest, Apply) { autoware::lidar_transfusion::NonMaximumSuppression nms; diff --git a/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.cpp b/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.cpp index bc30ca04ebfe6..b09e85f0a9325 100644 --- a/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.cpp +++ b/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.cpp @@ -17,6 +17,7 @@ #include #include +#include #include namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.cpp b/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.cpp index c4a1c37b4ff77..afc25024d122e 100644 --- a/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.cpp +++ b/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.cpp @@ -23,6 +23,7 @@ #include #include +#include #include namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/test/test_ros_utils.cpp b/perception/autoware_lidar_transfusion/test/test_ros_utils.cpp index ac9c4ca0378cb..31468b28f962a 100644 --- a/perception/autoware_lidar_transfusion/test/test_ros_utils.cpp +++ b/perception/autoware_lidar_transfusion/test/test_ros_utils.cpp @@ -16,6 +16,9 @@ #include +#include +#include + TEST(TestSuite, box3DToDetectedObject) { std::vector class_names = {"CAR", "TRUCK", "BUS", "TRAILER", diff --git a/perception/autoware_lidar_transfusion/test/test_voxel_generator.cpp b/perception/autoware_lidar_transfusion/test/test_voxel_generator.cpp index df546dbde6d97..3eccc4eac1e83 100644 --- a/perception/autoware_lidar_transfusion/test/test_voxel_generator.cpp +++ b/perception/autoware_lidar_transfusion/test/test_voxel_generator.cpp @@ -22,6 +22,9 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" +#include +#include + namespace autoware::lidar_transfusion { diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 4c4b7b30caecf..42dbf5b83fa8c 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -46,8 +46,13 @@ #include #include #include +#include #include #include +#include +#include +#include +#include namespace autoware::map_based_prediction { @@ -691,7 +696,7 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) if ( object.kinematics.orientation_availability == - autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + autoware_perception_msgs::msg::TrackedObjectKinematics::AVAILABLE) { return; } @@ -712,7 +717,7 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) if (abs_object_speed < min_abs_speed) return; switch (object.kinematics.orientation_availability) { - case autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN: { + case autoware_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN: { const auto original_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); // flip the angle diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index 57104082d0cf7..3bd6d62aba999 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -20,6 +20,9 @@ #include #include +#include +#include +#include namespace autoware::map_based_prediction { diff --git a/perception/autoware_map_based_prediction/src/predictor_vru.cpp b/perception/autoware_map_based_prediction/src/predictor_vru.cpp index 31630f7645171..0eea665cbd8a2 100644 --- a/perception/autoware_map_based_prediction/src/predictor_vru.cpp +++ b/perception/autoware_map_based_prediction/src/predictor_vru.cpp @@ -19,6 +19,15 @@ #include #include +#include +#include +#include +#include +#include +#include +#include +#include + namespace autoware::map_based_prediction { using autoware::universe_utils::ScopedTimeTrack; diff --git a/perception/autoware_map_based_prediction/src/utils.cpp b/perception/autoware_map_based_prediction/src/utils.cpp index 8c4a25a793bf7..cfc5036106daf 100644 --- a/perception/autoware_map_based_prediction/src/utils.cpp +++ b/perception/autoware_map_based_prediction/src/utils.cpp @@ -22,6 +22,15 @@ #include #include +#include +#include +#include +#include +#include +#include +#include +#include + namespace autoware::map_based_prediction { namespace utils diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index daa8db5db91e6..24e2b0c9f5acf 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -20,6 +20,7 @@ #include #include +#include namespace { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 399634b63bffe..89258835f920b 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -28,6 +28,8 @@ #include #include +#include + namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index 6f63ecbdce06d..700800e94ecd5 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -276,7 +276,7 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons Eigen::MatrixXd X_next_t(DIM, 1); // predicted state X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VEL) * cos_yaw * dt; // dx = v * cos(yaw) X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VEL) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega + X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ))*dt; // dyaw = omega X_next_t(IDX::VEL) = X_t(IDX::VEL); X_next_t(IDX::WZ) = X_t(IDX::WZ); diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp index 3f0b854931842..ddfdc7ef7cb10 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -20,6 +20,8 @@ #include #include +#include + namespace autoware::multi_object_tracker { namespace uncertainty diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp index 59f50bd2e9f0b..bc27525d85f55 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp @@ -20,8 +20,11 @@ #include #include +#include #include #include +#include +#include namespace { diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp index c77b465fb2bc9..e564afc9fd9d0 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp @@ -77,7 +77,6 @@ class TrackerObjectDebugger std::vector object_data_list_; std::list unused_marker_ids_; - int32_t marker_id_ = 0; std::vector> object_data_groups_; std::vector channel_names_; diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp index 3e47bbe9bed8d..9d830bb9e5b81 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -14,7 +14,10 @@ #include "debugger.hpp" +#include #include +#include +#include namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 68a356cd91da5..a8e27ed4f0146 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #include #include @@ -213,7 +214,17 @@ void MultiObjectTracker::onTrigger() ObjectsList objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (!is_objects_ready) return; - onMessage(objects_list); + + // process start + last_updated_time_ = current_time; + const rclcpp::Time latest_time(objects_list.back().second.header.stamp); + debugger_->startMeasurementTime(this->now(), latest_time); + // run process for each DetectedObjects + for (const auto & objects_data : objects_list) { + runProcess(objects_data.second, objects_data.first); + } + // process end + debugger_->endMeasurementTime(this->now()); // Publish without delay compensation if (!publish_timer_) { @@ -245,22 +256,6 @@ void MultiObjectTracker::onTimer() if (should_publish) checkAndPublish(current_time); } -void MultiObjectTracker::onMessage(const ObjectsList & objects_list) -{ - const rclcpp::Time current_time = this->now(); - const rclcpp::Time oldest_time(objects_list.front().second.header.stamp); - last_updated_time_ = current_time; - - // process start - debugger_->startMeasurementTime(this->now(), oldest_time); - // run process for each DetectedObjects - for (const auto & objects_data : objects_list) { - runProcess(objects_data.second, objects_data.first); - } - // process end - debugger_->endMeasurementTime(this->now()); -} - void MultiObjectTracker::runProcess( const DetectedObjects & input_objects, const uint & channel_index) { diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index b9886ee3fc847..79a8c1b98dcca 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -98,7 +98,6 @@ class MultiObjectTracker : public rclcpp::Node // callback functions void onTimer(); void onTrigger(); - void onMessage(const ObjectsList & objects_list); // publish processes void runProcess(const DetectedObjects & input_objects, const uint & channel_index); diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index c9ee8dae9562a..fa333ea06a4b9 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -17,6 +17,8 @@ #include #include +#include +#include namespace autoware::multi_object_tracker { @@ -144,22 +146,22 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim } void InputStream::getObjectsOlderThan( - const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, + const rclcpp::Time & object_latest_time, const rclcpp::Time & object_earliest_time, ObjectsList & objects_list) { - if (object_latest_time < object_oldest_time) { + if (object_latest_time < object_earliest_time) { RCLCPP_WARN( node_.get_logger(), "InputManager::getObjectsOlderThan %s: Invalid object time interval, object_latest_time: %f, " - "object_oldest_time: %f", - long_name_.c_str(), object_latest_time.seconds(), object_oldest_time.seconds()); + "object_earliest_time: %f", + long_name_.c_str(), object_latest_time.seconds(), object_earliest_time.seconds()); return; } for (const auto & objects : objects_que_) { const rclcpp::Time object_time = rclcpp::Time(objects.header.stamp); // ignore objects older than the specified duration - if (object_time < object_oldest_time) { + if (object_time < object_earliest_time) { continue; } @@ -238,7 +240,7 @@ void InputManager::onTrigger(const uint & index) const void InputManager::getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, - rclcpp::Time & object_oldest_time) const + rclcpp::Time & object_earliest_time) const { // Set the object time interval @@ -257,19 +259,19 @@ void InputManager::getObjectTimeInterval( object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time; } - // 2. object_oldest_time - // The default object_oldest_time is to have a 1-second time interval - const rclcpp::Time object_oldest_time_default = + // 2. object_earliest_time + // The default object_earliest_time is to have a 1-second time interval + const rclcpp::Time object_earliest_time_default = object_latest_time - rclcpp::Duration::from_seconds(1.0); - if (latest_exported_object_time_ < object_oldest_time_default) { + if (latest_exported_object_time_ < object_earliest_time_default) { // if the latest exported object time is too old, set to the default - object_oldest_time = object_oldest_time_default; + object_earliest_time = object_earliest_time_default; } else if (latest_exported_object_time_ > object_latest_time) { // if the latest exported object time is newer than the object_latest_time, set to the default - object_oldest_time = object_oldest_time_default; + object_earliest_time = object_earliest_time_default; } else { - // The object_oldest_time is the latest exported object time - object_oldest_time = latest_exported_object_time_; + // The object_earliest_time is the latest exported object time + object_earliest_time = latest_exported_object_time_; } } @@ -320,8 +322,8 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li // Get the time interval for the objects rclcpp::Time object_latest_time; - rclcpp::Time object_oldest_time; - getObjectTimeInterval(now, object_latest_time, object_oldest_time); + rclcpp::Time object_earliest_time; + getObjectTimeInterval(now, object_latest_time, object_earliest_time); // Optimize the target stream, latency, and its band // The result will be used for the next time, so the optimization is after getting the time @@ -331,7 +333,7 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li // Get objects from all input streams // adds up to the objects vector for efficient processing for (const auto & input_stream : input_streams_) { - input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_list); + input_stream->getObjectsOlderThan(object_latest_time, object_earliest_time, objects_list); } // Sort objects by timestamp @@ -363,7 +365,7 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li RCLCPP_DEBUG( node_.get_logger(), "InputManager::getObjects No objects in the object list, object time band from %f to %f", - (now - object_oldest_time).seconds(), (now - object_latest_time).seconds()); + (now - object_earliest_time).seconds(), (now - object_latest_time).seconds()); } } diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index f8b221f733faa..01b3148251743 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -57,7 +57,7 @@ class InputStream bool isTimeInitialized() const { return initial_count_ > 0; } uint getIndex() const { return index_; } void getObjectsOlderThan( - const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, + const rclcpp::Time & object_latest_time, const rclcpp::Time & object_earliest_time, ObjectsList & objects_list); bool isSpawnEnabled() const { return is_spawn_enabled_; } @@ -88,7 +88,6 @@ class InputStream // bool is_time_initialized_{false}; int initial_count_{0}; - double expected_interval_{}; double latency_mean_{}; double latency_var_{}; double interval_mean_{}; @@ -130,13 +129,11 @@ class InputManager double target_stream_latency_std_{0.04}; // [s] double target_stream_interval_{0.1}; // [s] double target_stream_interval_std_{0.02}; // [s] - double target_latency_{0.2}; // [s] - double target_latency_band_{1.0}; // [s] private: void getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, - rclcpp::Time & object_oldest_time) const; + rclcpp::Time & object_earliest_time) const; void optimizeTimings(); }; diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index ba9b3d17fd20f..f4202c1f6e413 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -20,6 +20,10 @@ #include "autoware_perception_msgs/msg/tracked_objects.hpp" #include +#include +#include +#include +#include namespace autoware::multi_object_tracker { diff --git a/perception/autoware_object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp index af9d0e15f4894..0e2b88f4aa566 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -25,8 +25,12 @@ #include #include +#include +#include +#include #include #include +#include using Label = autoware_perception_msgs::msg::ObjectClassification; diff --git a/perception/autoware_radar_crossing_objects_noise_filter/README.md b/perception/autoware_radar_crossing_objects_noise_filter/README.md index 553d932717442..4039624217a5c 100644 --- a/perception/autoware_radar_crossing_objects_noise_filter/README.md +++ b/perception/autoware_radar_crossing_objects_noise_filter/README.md @@ -10,7 +10,7 @@ This package can filter the noise objects which cross to the ego vehicle. This package aim to filter the noise objects which cross from the ego vehicle. The reason why these objects are noise is as below. -- 1. The objects with doppler velocity can be trusted more than those with vertical velocity to it. +#### 1. The objects with doppler velocity can be trusted more than those with vertical velocity to it Radars can get velocity information of objects as doppler velocity, but cannot get vertical velocity to doppler velocity directory. Some radars can output the objects with not only doppler velocity but also vertical velocity by estimation. @@ -22,7 +22,7 @@ Velocity estimation fails on static objects, resulting in ghost objects crossing ![vertical_velocity_objects](docs/vertical_velocity_objects.png) -- 2. Turning around by ego vehicle affect the output from radar. +#### 2. Turning around by ego vehicle affect the output from radar When the ego vehicle turns around, the radars outputting at the object level sometimes fail to estimate the twist of objects correctly even if [radar_tracks_msgs_converter](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_radar_tracks_msgs_converter) compensates by the ego vehicle twist. So if an object detected by radars has circular motion viewing from base_link, it is likely that the speed is estimated incorrectly and that the object is a static object. diff --git a/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp index 93475af7b3628..9a0477befd5be 100644 --- a/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp +++ b/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp @@ -20,6 +20,8 @@ #include +#include + std::shared_ptr get_node(double angle_threshold, double velocity_threshold) { diff --git a/perception/autoware_radar_object_clustering/README.md b/perception/autoware_radar_object_clustering/README.md index 0e66208d9f84e..cf52c33eba274 100644 --- a/perception/autoware_radar_object_clustering/README.md +++ b/perception/autoware_radar_object_clustering/README.md @@ -17,12 +17,12 @@ Therefore, by this package the multiple detection results are clustered into one ### Algorithm -- 1. Sort by distance from `base_link` +#### 1. Sort by distance from `base_link` At first, to prevent changing the result from depending on the order of objects in DetectedObjects, input objects are sorted by distance from `base_link`. In addition, to apply matching in closeness order considering occlusion, objects are sorted in order of short distance in advance. -- 2. Clustering +#### 2. Clustering If two radar objects are near, and yaw angle direction and velocity between two radar objects is similar (the degree of these is defined by parameters), then these are clustered. Note that radar characteristic affect parameters for this matching. @@ -32,13 +32,13 @@ For example, if resolution of range distance or angle is low and accuracy of vel After grouping for all radar objects, if multiple radar objects are grouping, the kinematics of the new clustered object is calculated from average of that and label and shape of the new clustered object is calculated from top confidence in radar objects. -- 3. Fixed label correction +#### 3. Fixed label correction When the label information from radar outputs lack accuracy, `is_fixed_label` parameter is recommended to set `true`. If the parameter is true, the label of a clustered object is overwritten by the label set by `fixed_label` parameter. If this package use for faraway dynamic object detection with radar, the parameter is recommended to set to `VEHICLE`. -- 4. Fixed size correction +#### 4. Fixed size correction When the size information from radar outputs lack accuracy, `is_fixed_size` parameter is recommended to set `true`. If the parameter is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters. diff --git a/perception/autoware_radar_object_tracker/src/association/data_association.cpp b/perception/autoware_radar_object_tracker/src/association/data_association.cpp index 673ae084fb242..79c91a96c4cc8 100644 --- a/perception/autoware_radar_object_tracker/src/association/data_association.cpp +++ b/perception/autoware_radar_object_tracker/src/association/data_association.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index 47b9430616fc9..04b2bd24d4b8d 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -30,6 +30,9 @@ #include #include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index 6facac3707c56..80b965ddb3840 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -30,6 +30,9 @@ #include #include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/tracker_base.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/tracker_base.cpp index ce63d96dfb83a..69a020a774372 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/tracker_base.cpp @@ -20,6 +20,7 @@ #include #include +#include namespace autoware::radar_object_tracker { diff --git a/perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp index f0a6a5dfd384d..cfbffdc089c11 100644 --- a/perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp +++ b/perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp @@ -14,6 +14,10 @@ #include "autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp" +#include +#include +#include + namespace autoware::radar_object_tracker::utils { diff --git a/perception/autoware_radar_object_tracker/src/utils/utils.cpp b/perception/autoware_radar_object_tracker/src/utils/utils.cpp index 351c4cfd21159..9ea4f0a36a9ac 100644 --- a/perception/autoware_radar_object_tracker/src/utils/utils.cpp +++ b/perception/autoware_radar_object_tracker/src/utils/utils.cpp @@ -14,6 +14,8 @@ #include "autoware_radar_object_tracker/utils/utils.hpp" +#include + namespace autoware::radar_object_tracker::utils { // concatenate matrices vertically diff --git a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp index 62b6b9f3b09b5..e52fc57a5a20c 100644 --- a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp +++ b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp @@ -22,6 +22,8 @@ #include +#include + namespace autoware::low_intensity_cluster_filter { LowIntensityClusterFilter::LowIntensityClusterFilter(const rclcpp::NodeOptions & node_options) diff --git a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp index 61e23860cd195..5f2dc05ba7eff 100644 --- a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp +++ b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp @@ -56,13 +56,8 @@ class LowIntensityClusterFilter : public rclcpp::Node double max_y_; double min_y_; - double max_x_transformed_; - double min_x_transformed_; - double max_y_transformed_; - double min_y_transformed_; // Eigen::Vector4f min_boundary_transformed_; // Eigen::Vector4f max_boundary_transformed_; - bool is_validation_range_transformed_ = false; const std::string base_link_frame_id_ = "base_link"; autoware::detected_object_validation::utils::FilterTargetLabel filter_target_; diff --git a/perception/autoware_raindrop_cluster_filter/test/test_low_intensity_cluster_filter.cpp b/perception/autoware_raindrop_cluster_filter/test/test_low_intensity_cluster_filter.cpp index e7644ce45e443..c53ca66db6720 100644 --- a/perception/autoware_raindrop_cluster_filter/test/test_low_intensity_cluster_filter.cpp +++ b/perception/autoware_raindrop_cluster_filter/test/test_low_intensity_cluster_filter.cpp @@ -24,6 +24,9 @@ #include +#include +#include +#include #include using autoware::low_intensity_cluster_filter::LowIntensityClusterFilter; diff --git a/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp index 072d3b661b8b3..b4b1f18832da7 100644 --- a/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp +++ b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp @@ -18,8 +18,13 @@ #include +#include #include +#include +#include #include +#include +#include namespace autoware::shape_estimation { @@ -140,7 +145,7 @@ void TrtShapeEstimator::preprocess(const DetectedObjectsWithFeature & input) } int iter_count = static_cast(input_pc_size) / point_size_of_cloud; - int remainer_count = static_cast(input_pc_size) % point_size_of_cloud; + int remaining_points_count = static_cast(input_pc_size) % point_size_of_cloud; for (int j = 1; j < iter_count; j++) { for (int k = 0; k < point_size_of_cloud; k++) { @@ -155,7 +160,7 @@ void TrtShapeEstimator::preprocess(const DetectedObjectsWithFeature & input) } } - for (int j = 0; j < remainer_count; j++) { + for (int j = 0; j < remaining_points_count; j++) { input_pc_h_[i * input_chan * input_pc_size + 0 + iter_count * point_size_of_cloud + j] = input_pc_h_[i * input_chan * input_pc_size + j]; diff --git a/perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp b/perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp index df04ade05b1ce..a7554f8249785 100644 --- a/perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp +++ b/perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp @@ -22,6 +22,9 @@ #include +#include +#include + namespace { using autoware::shape_estimation::ShapeEstimationNode; diff --git a/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/calibrator.hpp b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/calibrator.hpp index 0977621894920..59833a348ba91 100644 --- a/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/calibrator.hpp +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/calibrator.hpp @@ -47,7 +47,9 @@ #include #include +#include #include +#include #include #include #include @@ -270,19 +272,19 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator output.write(reinterpret_cast(cache), length); } - double getQuantile() const noexcept + double getQuantile() const noexcept override { printf("Quantile %f\n", m_quantile); return m_quantile; } - double getRegressionCutoff(void) const noexcept + double getRegressionCutoff(void) const noexcept override { printf("Cutoff %f\n", m_cutoff); return m_cutoff; } - const void * readHistogramCache(std::size_t & length) noexcept + const void * readHistogramCache(std::size_t & length) noexcept override { hist_cache_.clear(); std::ifstream input(histogram_cache_file_, std::ios::binary); @@ -301,7 +303,7 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator } return length ? &hist_cache_[0] : nullptr; } - void writeHistogramCache(void const * ptr, std::size_t length) noexcept + void writeHistogramCache(void const * ptr, std::size_t length) noexcept override { std::ofstream output(histogram_cache_file_, std::ios::binary); output.write(reinterpret_cast(ptr), length); diff --git a/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp index 345e663441557..ee16343b956d1 100644 --- a/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp @@ -107,7 +107,6 @@ class TrtClassifier std::vector input_h_; CudaUniquePtr input_d_; - bool needs_output_decode_; size_t out_elem_num_; size_t out_elem_num_per_batch_; CudaUniquePtr out_prob_d_; diff --git a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp index 4ca70c2f75618..b24e4fe56e8b8 100644 --- a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp +++ b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp @@ -20,10 +20,12 @@ #include #include +#include #include #include #include #include +#include #include static void trimLeft(std::string & s) diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp index cf5ebdc525a89..5aa897983af72 100644 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp index 6fb90d1415244..2b3b3f02f315f 100644 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp @@ -17,6 +17,8 @@ #ifndef YOLOX_STANDALONE #include + +#include #endif #include @@ -222,8 +224,6 @@ class TrtCommon // NOLINT TrtUniquePtr context_; std::unique_ptr calibrator_; - nvinfer1::Dims input_dims_; - nvinfer1::Dims output_dims_; std::string precision_; BatchConfig batch_config_; size_t max_workspace_size_; diff --git a/perception/autoware_tensorrt_common/src/simple_profiler.cpp b/perception/autoware_tensorrt_common/src/simple_profiler.cpp index c538470711fb8..2bcc1c4f9da06 100644 --- a/perception/autoware_tensorrt_common/src/simple_profiler.cpp +++ b/perception/autoware_tensorrt_common/src/simple_profiler.cpp @@ -14,7 +14,10 @@ #include +#include #include +#include +#include namespace autoware { diff --git a/perception/autoware_tensorrt_common/src/tensorrt_common.cpp b/perception/autoware_tensorrt_common/src/tensorrt_common.cpp index 632501c0dc057..897010d22bb4b 100644 --- a/perception/autoware_tensorrt_common/src/tensorrt_common.cpp +++ b/perception/autoware_tensorrt_common/src/tensorrt_common.cpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace { diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/calibrator.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/calibrator.hpp index 3a2bac315a3d0..24698c1d90f72 100644 --- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/calibrator.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/calibrator.hpp @@ -48,7 +48,9 @@ #include #include +#include #include +#include #include #include #include @@ -248,19 +250,19 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator output.write(reinterpret_cast(cache), length); } - double getQuantile() const noexcept + double getQuantile() const noexcept override { printf("Quantile %f\n", quantile_); return quantile_; } - double getRegressionCutoff(void) const noexcept + double getRegressionCutoff(void) const noexcept override { printf("Cutoff %f\n", cutoff_); return cutoff_; } - const void * readHistogramCache(std::size_t & length) noexcept + const void * readHistogramCache(std::size_t & length) noexcept override { hist_cache_.clear(); std::ifstream input(histogram_cache_file_, std::ios::binary); @@ -279,7 +281,7 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator } return length ? &hist_cache_[0] : nullptr; } - void writeHistogramCache(void const * ptr, std::size_t length) noexcept + void writeHistogramCache(void const * ptr, std::size_t length) noexcept override { std::ofstream output(histogram_cache_file_, std::ios::binary); output.write(reinterpret_cast(ptr), length); diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp index 451af305a4410..06540f2b7cd19 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp @@ -31,6 +31,7 @@ #include #include #include +#include #include namespace diff --git a/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp b/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp index 919d68202a9a9..243e82c65dab9 100644 --- a/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp @@ -15,6 +15,8 @@ #include #include +#include + #if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) #include namespace fs = ::std::filesystem; diff --git a/perception/autoware_tracking_object_merger/src/association/data_association.cpp b/perception/autoware_tracking_object_merger/src/association/data_association.cpp index 771fcb5f1a484..461dfb8ef14ce 100644 --- a/perception/autoware_tracking_object_merger/src/association/data_association.cpp +++ b/perception/autoware_tracking_object_merger/src/association/data_association.cpp @@ -94,11 +94,11 @@ void DataAssociation::assign( const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment) { - std::vector> score(src.rows()); + std::vector> score(static_cast(src.rows())); for (int row = 0; row < src.rows(); ++row) { - score.at(row).resize(src.cols()); + score.at(static_cast(row)).resize(static_cast(src.cols())); for (int col = 0; col < src.cols(); ++col) { - score.at(row).at(col) = src(row, col); + score.at(static_cast(row)).at(static_cast(col)) = src(row, col); } } // Solve @@ -134,15 +134,17 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const autoware_perception_msgs::msg::TrackedObjects & objects0, const autoware_perception_msgs::msg::TrackedObjects & objects1) { - Eigen::MatrixXd score_matrix = - Eigen::MatrixXd::Zero(objects1.objects.size(), objects0.objects.size()); + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero( + static_cast(objects1.objects.size()), + static_cast(objects0.objects.size())); for (size_t objects1_idx = 0; objects1_idx < objects1.objects.size(); ++objects1_idx) { const auto & object1 = objects1.objects.at(objects1_idx); for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { const auto & object0 = objects0.objects.at(objects0_idx); const double score = calcScoreBetweenObjects(object0, object1); - score_matrix(objects1_idx, objects0_idx) = score; + score_matrix( + static_cast(objects1_idx), static_cast(objects0_idx)) = score; } } return score_matrix; @@ -159,7 +161,8 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const autoware_perception_msgs::msg::TrackedObjects & objects0, const std::vector & trackers) { - Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size()); + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero( + static_cast(trackers.size()), static_cast(objects0.objects.size())); for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) { const auto & object1 = trackers.at(trackers_idx).getObject(); @@ -167,7 +170,8 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const auto & object0 = objects0.objects.at(objects0_idx); const double score = calcScoreBetweenObjects(object0, object1); - score_matrix(trackers_idx, objects0_idx) = score; + score_matrix( + static_cast(trackers_idx), static_cast(objects0_idx)) = score; } } return score_matrix; diff --git a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp index 55d31a1b890cd..44912e2242eb2 100644 --- a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -28,7 +28,10 @@ #include #include +#include +#include #include +#include using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -57,7 +60,8 @@ Eigen::MatrixXd calcScoreMatrixForAssociation( const rclcpp::Time current_time = rclcpp::Time(objects0.header.stamp); // calc score matrix - Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size()); + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero( + static_cast(trackers.size()), static_cast(objects0.objects.size())); for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) { const auto & tracker_obj = trackers.at(trackers_idx); const auto & object1 = tracker_obj.getObject(); @@ -77,7 +81,8 @@ Eigen::MatrixXd calcScoreMatrixForAssociation( } else { score = data_association_map.at("lidar-radar")->calcScoreBetweenObjects(object0, object1); } - score_matrix(trackers_idx, objects0_idx) = score; + score_matrix( + static_cast(trackers_idx), static_cast(objects0_idx)) = score; } } return score_matrix; @@ -316,9 +321,11 @@ bool DecorativeTrackerMergerNode::decorativeMerger( // look for tracker for (int tracker_idx = 0; tracker_idx < static_cast(inner_tracker_objects_.size()); ++tracker_idx) { - auto & object0_state = inner_tracker_objects_.at(tracker_idx); + auto & object0_state = + inner_tracker_objects_.at(static_cast::size_type>(tracker_idx)); if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found and merge - const auto & object1 = objects1.at(direct_assignment.at(tracker_idx)); + const auto & object1 = + objects1.at(static_cast::size_type>(direct_assignment.at(tracker_idx))); // merge object1 into object0_state object0_state.updateState(input_sensor, current_time, object1); } else { // not found @@ -328,7 +335,7 @@ bool DecorativeTrackerMergerNode::decorativeMerger( } // look for new object for (int object1_idx = 0; object1_idx < static_cast(objects1.size()); ++object1_idx) { - const auto & object1 = objects1.at(object1_idx); + const auto & object1 = objects1.at(static_cast::size_type>(object1_idx)); if (reverse_assignment.find(object1_idx) != reverse_assignment.end()) { // found } else { // not found inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object1)); diff --git a/perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp b/perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp index 9fd5fda7a63bb..9b859113390af 100644 --- a/perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp +++ b/perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp @@ -16,6 +16,11 @@ #include "autoware/tracking_object_merger/utils/utils.hpp" +#include +#include +#include +#include + namespace autoware::tracking_object_merger { diff --git a/perception/autoware_tracking_object_merger/src/utils/utils.cpp b/perception/autoware_tracking_object_merger/src/utils/utils.cpp index 04d7f94f2886c..15d130b4fcd50 100644 --- a/perception/autoware_tracking_object_merger/src/utils/utils.cpp +++ b/perception/autoware_tracking_object_merger/src/utils/utils.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include diff --git a/perception/autoware_traffic_light_arbiter/src/signal_match_validator.cpp b/perception/autoware_traffic_light_arbiter/src/signal_match_validator.cpp index d964f8b2cd7ba..396ebd8816088 100644 --- a/perception/autoware_traffic_light_arbiter/src/signal_match_validator.cpp +++ b/perception/autoware_traffic_light_arbiter/src/signal_match_validator.cpp @@ -14,6 +14,12 @@ #include "autoware/traffic_light_arbiter/signal_match_validator.hpp" +#include +#include +#include +#include +#include + namespace util { using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; diff --git a/perception/autoware_traffic_light_arbiter/test/test_node.cpp b/perception/autoware_traffic_light_arbiter/test/test_node.cpp index 1419f8675d0fa..f993ad7cec84d 100644 --- a/perception/autoware_traffic_light_arbiter/test/test_node.cpp +++ b/perception/autoware_traffic_light_arbiter/test/test_node.cpp @@ -21,6 +21,10 @@ #include #include +#include +#include +#include +#include #include using autoware::TrafficLightArbiter; diff --git a/perception/autoware_traffic_light_classifier/src/classifier/classifier_interface.hpp b/perception/autoware_traffic_light_classifier/src/classifier/classifier_interface.hpp index cff5828a5b00f..a6b1f8413b1a3 100644 --- a/perception/autoware_traffic_light_classifier/src/classifier/classifier_interface.hpp +++ b/perception/autoware_traffic_light_classifier/src/classifier/classifier_interface.hpp @@ -27,6 +27,7 @@ namespace autoware::traffic_light class ClassifierInterface { public: + virtual ~ClassifierInterface() = default; virtual bool getTrafficSignals( const std::vector & input_image, tier4_perception_msgs::msg::TrafficLightArray & traffic_signals) = 0; diff --git a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp index 76f5e608ca425..37ffca4a9526c 100644 --- a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp +++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp @@ -14,6 +14,8 @@ #include "traffic_light_fine_detector_node.hpp" +#include + #if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) #include namespace fs = ::std::filesystem; diff --git a/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp b/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp index d133fe0b443d1..5e74d77df644f 100644 --- a/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp +++ b/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp @@ -31,6 +31,11 @@ #include #include +#include +#include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/perception/autoware_traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.cpp b/perception/autoware_traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.cpp index 7ae20e9c38089..5ade76fc8d8fa 100644 --- a/perception/autoware_traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.cpp +++ b/perception/autoware_traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include diff --git a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 7fd5ac8489226..751e80e101e84 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -16,6 +16,8 @@ #include "occlusion_predictor.hpp" #include +#include +#include namespace { diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp index 891011b8cac7a..c797399f07959 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp @@ -201,9 +201,9 @@ void TrafficLightRoiVisualizerNode::imageRoughRoiCallback( // bbox drawing cv_ptr = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::RGB8); for (auto tl_rough_roi : input_tl_rough_roi_msg->rois) { - // visualize rough roi - createRect(cv_ptr->image, tl_rough_roi, cv::Scalar(0, 255, 0)); - + // note: a signal will still be output even if it is undetected + // Its position and size will be set as 0 and the color will be set as unknown + // So a rough roi will always have correspond roi a correspond traffic signal ClassificationResult result; bool has_correspond_traffic_signal = getClassificationResult(tl_rough_roi.traffic_light_id, *input_traffic_signals_msg, result); @@ -211,6 +211,8 @@ void TrafficLightRoiVisualizerNode::imageRoughRoiCallback( bool has_correspond_roi = getRoiFromId(tl_rough_roi.traffic_light_id, input_tl_roi_msg, tl_roi); + createRect(cv_ptr->image, tl_rough_roi, extractShapeInfo(result.label).color); + if (has_correspond_roi && has_correspond_traffic_signal) { // has fine detection and classification results createRect(cv_ptr->image, tl_roi, result); diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp index 8c7a59cbcb003..14b8d2e51c78d 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -29,6 +30,10 @@ void drawShape( cv::Mat & image, const std::vector & params, int size, const cv::Point & position, const cv::Scalar & color, float probability) { + // skip if the roi position is set as (0,0), which means it is undetected + if (position.x == 0 && position.y == 0) { + return; + } // load concatenated shape image const auto shape_img = loadShapeImage(params, size); @@ -42,14 +47,13 @@ void drawShape( const int fill_rect_h = std::max(shape_img.rows, text_size.height) + 10; const cv::Point rect_position(position.x, position.y - fill_rect_h); - if ( rect_position.x < 0 || rect_position.y < 0 || rect_position.x + fill_rect_w > image.cols || position.y > image.rows) { // TODO(KhalilSelyan): This error message may flood the terminal logs, so commented out // temporarily. Need to consider a better way. - // std::cerr << "Adjusted position is out of image bounds." << std::endl; + std::cerr << "Adjusted position is out of image bounds." << std::endl; return; } cv::Rect rectangle(rect_position.x, rect_position.y, fill_rect_w, fill_rect_h); diff --git a/perception/perception_utils/src/run_length_encoder.cpp b/perception/perception_utils/src/run_length_encoder.cpp index fb7f5ba33b846..59430de5069e8 100644 --- a/perception/perception_utils/src/run_length_encoder.cpp +++ b/perception/perception_utils/src/run_length_encoder.cpp @@ -14,6 +14,8 @@ #include "perception_utils/run_length_encoder.hpp" +#include + namespace perception_utils { diff --git a/perception/perception_utils/test/test_utils.cpp b/perception/perception_utils/test/test_utils.cpp index d28b8489971e0..5d721c731935e 100644 --- a/perception/perception_utils/test/test_utils.cpp +++ b/perception/perception_utils/test/test_utils.cpp @@ -18,6 +18,8 @@ #include +#include + // Test case 1: Test if the decoded image is the same as the original image TEST(UtilsTest, runLengthEncoderDecoderTest) { diff --git a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp index 72b56f0348a40..4fed5bcb6cff3 100644 --- a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp @@ -139,6 +139,13 @@ class CostmapGenerator : public rclcpp::Node void set_current_pose(); + /// \brief set position of grid center + /// \details computes relative position of ego from current grid center, + /// if offset is larger than grid resolution, grid center will be updated + /// by a multiple of the grid resolution + /// \param[in] tf costmap frame to vehicle frame transform + void set_grid_center(const geometry_msgs::msg::TransformStamped & tf); + void onTimer(); bool isActive(); @@ -148,7 +155,9 @@ class CostmapGenerator : public rclcpp::Node /// \brief publish ros msg: grid_map::GridMap, and nav_msgs::OccupancyGrid /// \param[in] gridmap with calculated cost - void publishCostmap(const grid_map::GridMap & costmap); + /// \param[in] tf costmap frame to vehicle frame transform + void publishCostmap( + const grid_map::GridMap & costmap, const geometry_msgs::msg::TransformStamped & tf); /// \brief fill a vector with road area polygons /// \param [in] lanelet_map input lanelet map diff --git a/planning/autoware_costmap_generator/src/costmap_generator.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp index d791836fa0a08..052bb2728a3b1 100644 --- a/planning/autoware_costmap_generator/src/costmap_generator.cpp +++ b/planning/autoware_costmap_generator/src/costmap_generator.cpp @@ -286,11 +286,7 @@ void CostmapGenerator::onTimer() } time_keeper_->end_track("lookupTransform"); - // Set grid center - grid_map::Position p; - p.x() = tf.transform.translation.x; - p.y() = tf.transform.translation.y; - costmap_.setPosition(p); + set_grid_center(tf); if ((param_->use_wayarea || param_->use_parkinglot) && lanelet_map_) { autoware::universe_utils::ScopedTimeTrack st("generatePrimitivesCostmap()", *time_keeper_); @@ -312,7 +308,18 @@ void CostmapGenerator::onTimer() costmap_[LayerName::combined] = generateCombinedCostmap(); } - publishCostmap(costmap_); + publishCostmap(costmap_, tf); +} + +void CostmapGenerator::set_grid_center(const geometry_msgs::msg::TransformStamped & tf) +{ + const auto cur_pos = costmap_.getPosition(); + const grid_map::Position ref_pos(tf.transform.translation.x, tf.transform.translation.y); + const auto disp = ref_pos - cur_pos; + const auto resolution = costmap_.getResolution(); + const grid_map::Position offset( + std::round(disp.x() / resolution) * resolution, std::round(disp.y() / resolution) * resolution); + costmap_.setPosition(cur_pos + offset); } bool CostmapGenerator::isActive() @@ -324,8 +331,8 @@ bool CostmapGenerator::isActive() if (param_->activate_by_scenario) { if (!scenario_) return false; const auto & s = scenario_->activating_scenarios; - return std::any_of(s.begin(), s.end(), [](const auto scen) { - return scen == tier4_planning_msgs::msg::Scenario::PARKING; + return std::any_of(s.begin(), s.end(), [](const auto scenario) { + return scenario == tier4_planning_msgs::msg::Scenario::PARKING; }); } @@ -452,7 +459,8 @@ grid_map::Matrix CostmapGenerator::generateCombinedCostmap() return combined_costmap[LayerName::combined]; } -void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap) +void CostmapGenerator::publishCostmap( + const grid_map::GridMap & costmap, const geometry_msgs::msg::TransformStamped & tf) { // Set header std_msgs::msg::Header header; @@ -465,11 +473,13 @@ void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap) costmap, LayerName::combined, param_->grid_min_value, param_->grid_max_value, out_occupancy_grid); out_occupancy_grid.header = header; + out_occupancy_grid.info.origin.position.z = tf.transform.translation.z; pub_occupancy_grid_->publish(out_occupancy_grid); // Publish GridMap auto out_gridmap_msg = grid_map::GridMapRosConverter::toMessage(costmap); out_gridmap_msg->header = header; + out_gridmap_msg->info.pose.position.z = tf.transform.translation.z; pub_costmap_->publish(*out_gridmap_msg); // Publish ProcessingTime diff --git a/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp b/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp index bcd2e15fe9ca7..64f558b4f5604 100644 --- a/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp @@ -50,6 +50,7 @@ #include #include +#include #include #include diff --git a/planning/autoware_costmap_generator/test/test_costmap_generator.cpp b/planning/autoware_costmap_generator/test/test_costmap_generator.cpp index 00c35e2514f43..f0db05b6fdd6f 100644 --- a/planning/autoware_costmap_generator/test/test_costmap_generator.cpp +++ b/planning/autoware_costmap_generator/test/test_costmap_generator.cpp @@ -20,6 +20,10 @@ #include +#include +#include +#include + using autoware::costmap_generator::CostmapGenerator; using tier4_planning_msgs::msg::Scenario; diff --git a/planning/autoware_costmap_generator/test/test_object_map_utils.cpp b/planning/autoware_costmap_generator/test/test_object_map_utils.cpp index a20c2326d2757..5225b089f845f 100644 --- a/planning/autoware_costmap_generator/test/test_object_map_utils.cpp +++ b/planning/autoware_costmap_generator/test/test_object_map_utils.cpp @@ -17,6 +17,8 @@ #include +#include + namespace { grid_map::GridMap construct_gridmap( diff --git a/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp index 4ab5a32e77028..83f7055531e7d 100644 --- a/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp @@ -19,6 +19,8 @@ #include #include +#include + namespace { geometry_msgs::msg::Point32 toPoint32(const geometry_msgs::msg::Pose & pose) diff --git a/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp index b98479f43c2aa..ecfeaaf6ac676 100644 --- a/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp @@ -16,6 +16,8 @@ #include +#include + namespace autoware::costmap_generator { using pointcloud = pcl::PointCloud; diff --git a/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index 2aa06079dadbc..6a8fae3bf394c 100644 --- a/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -22,7 +22,6 @@ #include #include -#include #include #include #include diff --git a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index 2052c8438bcfa..cf42763fc6b60 100644 --- a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -14,12 +14,12 @@ #include "autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" -#include +#include #include +#include #include #include #include -#include namespace autoware::external_velocity_limit_selector { @@ -34,12 +34,17 @@ VelocityLimit getHardestLimit( hardest_limit.max_velocity = node_param.max_vel; VelocityLimitConstraints normal_constraints{}; + normal_constraints.max_acceleration = node_param.normal.max_acc; normal_constraints.min_acceleration = node_param.normal.min_acc; normal_constraints.min_jerk = node_param.normal.min_jerk; normal_constraints.max_jerk = node_param.normal.max_jerk; double hardest_max_velocity = node_param.max_vel; double hardest_max_jerk = 0.0; + double hardest_max_acceleration = 0.0; + std::string hardest_max_acceleration_key; + size_t constraint_num = 0; + size_t acceleration_constraint_num = 0; for (const auto & limit : velocity_limits) { // guard nan, inf @@ -59,6 +64,19 @@ VelocityLimit getHardestLimit( ? limit.second.constraints : normal_constraints; + if (limit.second.use_constraints) { + constraint_num++; + if (limit.second.constraints.max_acceleration > normal_constraints.max_acceleration) { + acceleration_constraint_num++; + hardest_max_acceleration_key = limit.first; + } + } + + if (hardest_max_acceleration < limit.second.constraints.max_acceleration) { + hardest_max_acceleration_key = limit.first; + hardest_max_acceleration = limit.second.constraints.max_acceleration; + } + // find hardest jerk if (hardest_max_jerk < constraints.max_jerk) { hardest_limit.constraints = constraints; @@ -67,6 +85,14 @@ VelocityLimit getHardestLimit( } } + if (constraint_num > 0 && constraint_num == acceleration_constraint_num) { + if (velocity_limits.find(hardest_max_acceleration_key) != velocity_limits.end()) { + const auto constraints = velocity_limits.at(hardest_max_acceleration_key).constraints; + hardest_limit.constraints = constraints; + hardest_limit.use_constraints = true; + } + } + return hardest_limit; } diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp index 4a7f3b1cd6c2b..1f2347958cfd2 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp @@ -58,13 +58,15 @@ size_t get_next_target_index( const size_t current_target_index); Trajectory get_partial_trajectory( - const Trajectory & trajectory, const size_t start_index, const size_t end_index); + const Trajectory & trajectory, const size_t start_index, const size_t end_index, + const rclcpp::Clock::SharedPtr clock); Trajectory create_trajectory( const PoseStamped & current_pose, const PlannerWaypoints & planner_waypoints, const double & velocity); -Trajectory create_stop_trajectory(const PoseStamped & current_pose); +Trajectory create_stop_trajectory( + const PoseStamped & current_pose, const rclcpp::Clock::SharedPtr clock); Trajectory create_stop_trajectory(const Trajectory & trajectory); diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index f968d668223e0..a937114e653c6 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -168,8 +168,8 @@ bool FreespacePlannerNode::checkCurrentTrajectoryCollision() const size_t nearest_index_partial = autoware::motion_utils::findNearestIndex( partial_trajectory_.points, current_pose_.pose.position); const size_t end_index_partial = partial_trajectory_.points.size() - 1; - const auto forward_trajectory = - utils::get_partial_trajectory(partial_trajectory_, nearest_index_partial, end_index_partial); + const auto forward_trajectory = utils::get_partial_trajectory( + partial_trajectory_, nearest_index_partial, end_index_partial, get_clock()); const bool is_obs_found = algo_->hasObstacleOnTrajectory(utils::trajectory_to_pose_array(forward_trajectory)); @@ -316,7 +316,7 @@ void FreespacePlannerNode::onTimer() // stops. if (!is_new_parking_cycle_) { const auto stop_trajectory = partial_trajectory_.points.empty() - ? utils::create_stop_trajectory(current_pose_) + ? utils::create_stop_trajectory(current_pose_, get_clock()) : utils::create_stop_trajectory(partial_trajectory_); trajectory_pub_->publish(stop_trajectory); debug_pose_array_pub_->publish(utils::trajectory_to_pose_array(stop_trajectory)); @@ -352,7 +352,7 @@ void FreespacePlannerNode::onTimer() // Update partial trajectory updateTargetIndex(); partial_trajectory_ = - utils::get_partial_trajectory(trajectory_, prev_target_index_, target_index_); + utils::get_partial_trajectory(trajectory_, prev_target_index_, target_index_, get_clock()); // Publish messages trajectory_pub_->publish(partial_trajectory_); diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp index ef22a21d33eb1..a08c62723c3d5 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp @@ -16,6 +16,9 @@ #include +#include +#include + namespace autoware::freespace_planner::utils { @@ -87,11 +90,12 @@ size_t get_next_target_index( } Trajectory get_partial_trajectory( - const Trajectory & trajectory, const size_t start_index, const size_t end_index) + const Trajectory & trajectory, const size_t start_index, const size_t end_index, + const rclcpp::Clock::SharedPtr clock) { Trajectory partial_trajectory; partial_trajectory.header = trajectory.header; - partial_trajectory.header.stamp = rclcpp::Clock().now(); + partial_trajectory.header.stamp = clock->now(); partial_trajectory.points.reserve(trajectory.points.size()); for (size_t i = start_index; i <= end_index; ++i) { @@ -134,12 +138,13 @@ Trajectory create_trajectory( return trajectory; } -Trajectory create_stop_trajectory(const PoseStamped & current_pose) +Trajectory create_stop_trajectory( + const PoseStamped & current_pose, const rclcpp::Clock::SharedPtr clock) { PlannerWaypoints waypoints; PlannerWaypoint waypoint; - waypoints.header.stamp = rclcpp::Clock().now(); + waypoints.header.stamp = clock->now(); waypoints.header.frame_id = current_pose.header.frame_id; waypoint.pose.header = waypoints.header; waypoint.pose.pose = current_pose.pose; diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner.cpp index 37ea52c07734b..bccf417503edb 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner.cpp @@ -25,6 +25,11 @@ #include #include +#include +#include +#include +#include + using autoware::freespace_planner::FreespacePlannerNode; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -134,7 +139,8 @@ class TestFreespacePlanner : public ::testing::Test freespace_planner_->reversing_indices_ = reversing_indices; freespace_planner_->partial_trajectory_ = autoware::freespace_planner::utils::get_partial_trajectory( - trajectory_, 0, reversing_indices.front()); + trajectory_, 0, reversing_indices.front(), + std::make_shared(RCL_SYSTEM_TIME)); freespace_planner_->current_pose_.pose = trajectory_.points.front().pose; if (colliding) { @@ -161,7 +167,8 @@ class TestFreespacePlanner : public ::testing::Test trajectory_.points.size(), reversing_indices, freespace_planner_->prev_target_index_); freespace_planner_->partial_trajectory_ = autoware::freespace_planner::utils::get_partial_trajectory( - trajectory_, freespace_planner_->prev_target_index_, freespace_planner_->target_index_); + trajectory_, freespace_planner_->prev_target_index_, freespace_planner_->target_index_, + std::make_shared(RCL_SYSTEM_TIME)); Odometry odom; odom.pose.pose = trajectory_.points.front().pose; diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp index f43b4401865b2..8bf30757aafb7 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -20,6 +20,7 @@ #include +#include #include using autoware::freespace_planner::FreespacePlannerNode; diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp index 7c795a8eb7b85..e374c4056df1e 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp @@ -19,6 +19,7 @@ #include #include +#include #include using autoware::freespace_planner::utils::Odometry; @@ -229,9 +230,8 @@ TEST(FreespacePlannerUtilsTest, testGetPartialTrajectory) autoware::freespace_planner::utils::get_reversing_indices(trajectory); ASSERT_EQ(reversing_indices.size(), 2ul); - auto partial_traj = autoware::freespace_planner::utils::get_partial_trajectory( - trajectory, 0ul, reversing_indices.front()); + trajectory, 0ul, reversing_indices.front(), std::make_shared(RCL_SYSTEM_TIME)); ASSERT_FALSE(partial_traj.points.empty()); auto expected_size = reversing_indices.front() + 1ul; EXPECT_EQ(partial_traj.points.size(), expected_size); @@ -239,7 +239,8 @@ TEST(FreespacePlannerUtilsTest, testGetPartialTrajectory) EXPECT_FLOAT_EQ(partial_traj.points.back().longitudinal_velocity_mps, 0.0); partial_traj = autoware::freespace_planner::utils::get_partial_trajectory( - trajectory, reversing_indices.front(), reversing_indices.back()); + trajectory, reversing_indices.front(), reversing_indices.back(), + std::make_shared(RCL_SYSTEM_TIME)); ASSERT_FALSE(partial_traj.points.empty()); expected_size = reversing_indices.back() - reversing_indices.front() + 1ul; EXPECT_EQ(partial_traj.points.size(), expected_size); @@ -271,7 +272,8 @@ TEST(FreespacePlannerUtilsTest, testCreateStopTrajectory) geometry_msgs::msg::PoseStamped current_pose; current_pose.pose.position.x = 1.0; - auto stop_traj = autoware::freespace_planner::utils::create_stop_trajectory(current_pose); + auto stop_traj = autoware::freespace_planner::utils::create_stop_trajectory( + current_pose, std::make_shared(RCL_SYSTEM_TIME)); EXPECT_EQ(stop_traj.points.size(), 1ul); if (!stop_traj.points.empty()) { EXPECT_DOUBLE_EQ(stop_traj.points.front().pose.position.x, 1.0); diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp index 8a4d6bf2e42b5..52b6b404ba6eb 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp @@ -158,8 +158,11 @@ class AbstractPlanningAlgorithm { public: AbstractPlanningAlgorithm( - const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape) - : planner_common_param_(planner_common_param), collision_vehicle_shape_(collision_vehicle_shape) + const PlannerCommonParam & planner_common_param, const rclcpp::Clock::SharedPtr & clock, + const VehicleShape & collision_vehicle_shape) + : planner_common_param_(planner_common_param), + collision_vehicle_shape_(collision_vehicle_shape), + clock_(clock) { planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1); collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio; @@ -168,8 +171,11 @@ class AbstractPlanningAlgorithm AbstractPlanningAlgorithm( const PlannerCommonParam & planner_common_param, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) - : planner_common_param_(planner_common_param), collision_vehicle_shape_(vehicle_info, margin) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const rclcpp::Clock::SharedPtr & clock, const double margin = 0.0) + : planner_common_param_(planner_common_param), + collision_vehicle_shape_(vehicle_info, margin), + clock_(clock) { planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1); collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio; @@ -270,6 +276,9 @@ class AbstractPlanningAlgorithm PlannerCommonParam planner_common_param_; VehicleShape collision_vehicle_shape_; + // Pointer to the parent Node + rclcpp::Clock::SharedPtr clock_; + // costmap as occupancy grid nav_msgs::msg::OccupancyGrid costmap_; diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp index 65ef53d820cba..cd4ad9d4d6413 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp @@ -98,6 +98,9 @@ class AstarSearch : public AbstractPlanningAlgorithm const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape, const AstarParam & astar_param); + AstarSearch( + const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape, + const AstarParam & astar_param, const rclcpp::Clock::SharedPtr & clock); AstarSearch( const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape, rclcpp::Node & node) @@ -113,7 +116,8 @@ class AstarSearch : public AbstractPlanningAlgorithm node.declare_parameter("astar.distance_heuristic_weight"), node.declare_parameter("astar.smoothness_weight"), node.declare_parameter("astar.obstacle_distance_weight"), - node.declare_parameter("astar.goal_lat_distance_weight")}) + node.declare_parameter("astar.goal_lat_distance_weight")}, + node.get_clock()) { } diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp index 1bdc7105067d4..8f0ff9a677ebd 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp @@ -39,7 +39,7 @@ class RRTStar : public AbstractPlanningAlgorithm public: RRTStar( const PlannerCommonParam & planner_common_param, const VehicleShape & original_vehicle_shape, - const RRTStarParam & rrtstar_param); + const RRTStarParam & rrtstar_param, const rclcpp::Clock::SharedPtr & clock); RRTStar( const PlannerCommonParam & planner_common_param, const VehicleShape & original_vehicle_shape, @@ -51,7 +51,8 @@ class RRTStar : public AbstractPlanningAlgorithm node.declare_parameter("rrtstar.use_informed_sampling"), node.declare_parameter("rrtstar.max_planning_time"), node.declare_parameter("rrtstar.neighbor_radius"), - node.declare_parameter("rrtstar.margin")}) + node.declare_parameter("rrtstar.margin")}, + node.get_clock()) { } diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index f2003e438f5bf..7808d18f24aed 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -24,6 +24,9 @@ #include #include +#include +#include + namespace autoware::freespace_planning_algorithms { struct PlannerWaypointsVector diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index b968e70bd2c01..56d6526e26272 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include #include diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 875d44847b40c..0a17b112eed6f 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -24,6 +24,9 @@ #include #include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -67,7 +70,34 @@ Pose calcRelativePose(const Pose & base_pose, const Pose & pose) AstarSearch::AstarSearch( const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape, const AstarParam & astar_param) -: AbstractPlanningAlgorithm(planner_common_param, collision_vehicle_shape), +: AbstractPlanningAlgorithm( + planner_common_param, std::make_shared(RCL_ROS_TIME), collision_vehicle_shape), + astar_param_(astar_param), + goal_node_(nullptr), + use_reeds_shepp_(true) +{ + steering_resolution_ = + collision_vehicle_shape_.max_steering / planner_common_param_.turning_steps; + heading_resolution_ = 2.0 * M_PI / planner_common_param_.theta_size; + + const double avg_steering = + steering_resolution_ + (collision_vehicle_shape_.max_steering - steering_resolution_) / 2.0; + avg_turning_radius_ = + kinematic_bicycle_model::getTurningRadius(collision_vehicle_shape_.base_length, avg_steering); + + is_backward_search_ = astar_param_.search_method == "backward"; + + min_expansion_dist_ = astar_param_.expansion_distance; + max_expansion_dist_ = collision_vehicle_shape_.base_length * base_length_max_expansion_factor_; + + near_goal_dist_ = + std::max(astar_param.near_goal_distance, planner_common_param.longitudinal_goal_range); +} + +AstarSearch::AstarSearch( + const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape, + const AstarParam & astar_param, const rclcpp::Clock::SharedPtr & clock) +: AbstractPlanningAlgorithm(planner_common_param, clock, collision_vehicle_shape), astar_param_(astar_param), goal_node_(nullptr), use_reeds_shepp_(true) @@ -399,7 +429,7 @@ double AstarSearch::getLatDistanceCost(const Pose & pose) const void AstarSearch::setPath(const AstarNode & goal_node) { std_msgs::msg::Header header; - header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); + header.stamp = clock_->now(); header.frame_id = costmap_.header.frame_id; // From the goal node to the start node diff --git a/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp b/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp index ee6d1cb8d1d9a..7b5acb40892e2 100644 --- a/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp @@ -91,7 +91,7 @@ using autoware::freespace_planning_algorithms::ReedsSheppStateSpace; const double pi = M_PI; const double twopi = 2. * pi; -const double RS_EPS = 1e-6; +[[maybe_unused]] const double RS_EPS = 1e-6; // used only in assertions const double ZERO = 10 * std::numeric_limits::epsilon(); inline double mod2pi(double x) diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp index 696d04a06cb28..0427381db7de8 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp @@ -16,6 +16,8 @@ #include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" +#include + namespace autoware::freespace_planning_algorithms { rrtstar_core::Pose poseMsgToPose(const geometry_msgs::msg::Pose & pose_msg) @@ -26,13 +28,14 @@ rrtstar_core::Pose poseMsgToPose(const geometry_msgs::msg::Pose & pose_msg) RRTStar::RRTStar( const PlannerCommonParam & planner_common_param, const VehicleShape & original_vehicle_shape, - const RRTStarParam & rrtstar_param) + const RRTStarParam & rrtstar_param, const rclcpp::Clock::SharedPtr & clock) : AbstractPlanningAlgorithm( - planner_common_param, VehicleShape( - original_vehicle_shape.length + 2 * rrtstar_param.margin, - original_vehicle_shape.width + 2 * rrtstar_param.margin, - original_vehicle_shape.base_length, original_vehicle_shape.max_steering, - original_vehicle_shape.base2back + rrtstar_param.margin)), + planner_common_param, clock, + VehicleShape( + original_vehicle_shape.length + 2 * rrtstar_param.margin, + original_vehicle_shape.width + 2 * rrtstar_param.margin, original_vehicle_shape.base_length, + original_vehicle_shape.max_steering, + original_vehicle_shape.base2back + rrtstar_param.margin)), rrtstar_param_(rrtstar_param) { if (rrtstar_param_.margin <= 0) { @@ -129,7 +132,7 @@ bool RRTStar::hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & traj void RRTStar::setRRTPath(const std::vector & waypoints) { std_msgs::msg::Header header; - header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); + header.stamp = clock_->now(); header.frame_id = costmap_.header.frame_id; waypoints_.header = header; diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp index 853d9ef0a9787..6affc862cc64e 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp @@ -16,13 +16,16 @@ #include +#include #include #include #include #include #include +#include #include #include +#include // cspell: ignore rsspace // In this case, RSSpace means "Reeds Shepp state Space" diff --git a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 2b2254d66c844..c343a8773f8ad 100644 --- a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include #include #include @@ -231,7 +233,9 @@ std::unique_ptr configure_astar(bool use_multi) obstacle_distance_weight, goal_lat_distance_weight}; - auto algo = std::make_unique(planner_common_param, vehicle_shape, astar_param); + auto clock_ptr = std::make_shared(RCL_ROS_TIME); + auto algo = + std::make_unique(planner_common_param, vehicle_shape, astar_param, clock_ptr); return algo; } @@ -244,7 +248,10 @@ std::unique_ptr configure_rrtstar(bool informed, const double margin = 0.2; const double max_planning_time = 200; const auto rrtstar_param = fpa::RRTStarParam{update, informed, max_planning_time, mu, margin}; - auto algo = std::make_unique(planner_common_param, vehicle_shape, rrtstar_param); + + auto clock_ptr = std::make_shared(RCL_ROS_TIME); + auto algo = + std::make_unique(planner_common_param, vehicle_shape, rrtstar_param, clock_ptr); return algo; } diff --git a/planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp index 40181884d36c7..a7a66123b7856 100644 --- a/planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp @@ -18,6 +18,7 @@ #include #include +#include namespace autoware::freespace_planning_algorithms::rrtstar_core { diff --git a/planning/autoware_mission_planner/launch/mission_planner.launch.xml b/planning/autoware_mission_planner/launch/mission_planner.launch.xml index 8d77e417a6379..0d9c9307d549c 100644 --- a/planning/autoware_mission_planner/launch/mission_planner.launch.xml +++ b/planning/autoware_mission_planner/launch/mission_planner.launch.xml @@ -23,6 +23,6 @@ - + diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp index 08345282a9264..40bccd118965f 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -23,6 +23,7 @@ #include #include +#include namespace autoware::mission_planner::lanelet2 { diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp index 467bc33d20a56..cbbcfb84d2cbb 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -27,7 +27,10 @@ #include #include +#include +#include #include +#include namespace autoware::mission_planner { @@ -81,6 +84,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) // otherwise the mission planner rejects the request for the API. const auto period = rclcpp::Rate(10).period(); data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); }); + is_mission_planner_ready_ = false; logger_configure_ = std::make_unique(this); pub_processing_time_ = @@ -122,6 +126,7 @@ void MissionPlanner::check_initialization() } // All data is ready. Now API is available. + is_mission_planner_ready_ = true; RCLCPP_DEBUG(logger, "Route API is ready."); change_state(RouteState::UNSET); @@ -185,12 +190,8 @@ void MissionPlanner::on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr RCLCPP_ERROR(get_logger(), "The route hasn't set yet. Cannot reroute."); return; } - if (!planner_->ready()) { - RCLCPP_ERROR(get_logger(), "The planner is not ready."); - return; - } - if (!odometry_) { - RCLCPP_ERROR(get_logger(), "The vehicle pose is not received."); + if (!is_mission_planner_ready_) { + RCLCPP_ERROR(get_logger(), "The mission planner is not ready."); return; } if (!current_route_) { @@ -220,6 +221,12 @@ void MissionPlanner::on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr void MissionPlanner::on_clear_route( const ClearRoute::Request::SharedPtr, const ClearRoute::Response::SharedPtr res) { + if (!is_mission_planner_ready_) { + using ResponseCode = autoware_adapi_v1_msgs::msg::ResponseStatus; + throw service_utils::ServiceException( + ResponseCode::NO_EFFECT, "The mission planner is not ready.", true); + } + change_route(); change_state(RouteState::UNSET); res->status.success = true; @@ -235,13 +242,9 @@ void MissionPlanner::on_set_lanelet_route( throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state."); } - if (!planner_->ready()) { + if (!is_mission_planner_ready_) { throw service_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); - } - if (!odometry_) { - throw service_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + ResponseCode::ERROR_PLANNER_UNREADY, "The mission planner is not ready."); } if (is_reroute && !operation_mode_state_) { throw service_utils::ServiceException( @@ -302,13 +305,9 @@ void MissionPlanner::on_set_waypoint_route( throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state."); } - if (!planner_->ready()) { - throw service_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); - } - if (!odometry_) { + if (!is_mission_planner_ready_) { throw service_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + ResponseCode::ERROR_PLANNER_UNREADY, "The mission planner is not ready."); } if (is_reroute && !operation_mode_state_) { throw service_utils::ServiceException( diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp index b0389c49bef33..987ca6482ec11 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -136,6 +136,7 @@ class MissionPlanner : public rclcpp::Node rclcpp::TimerBase::SharedPtr data_check_timer_; void check_initialization(); + bool is_mission_planner_ready_; double reroute_time_threshold_; double minimum_reroute_length_; diff --git a/planning/autoware_mission_planner/src/mission_planner/service_utils.hpp b/planning/autoware_mission_planner/src/mission_planner/service_utils.hpp index b12b5f635275c..8b6e5c9916705 100644 --- a/planning/autoware_mission_planner/src/mission_planner/service_utils.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/service_utils.hpp @@ -19,6 +19,7 @@ #include +#include #include #include diff --git a/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp b/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp index 124a04e3e4ed2..8da26ab851bb4 100644 --- a/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp +++ b/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp @@ -29,6 +29,10 @@ #include #include +#include +#include +#include + using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::createQuaternionFromRPY; using autoware_planning_msgs::msg::LaneletRoute; diff --git a/planning/autoware_mission_planner/test/test_utility_functions.cpp b/planning/autoware_mission_planner/test/test_utility_functions.cpp index 6cde09b7664a2..da44a2ad17cf0 100644 --- a/planning/autoware_mission_planner/test/test_utility_functions.cpp +++ b/planning/autoware_mission_planner/test/test_utility_functions.cpp @@ -23,6 +23,8 @@ #include #include +#include + using autoware::mission_planner::lanelet2::convert_linear_ring_to_polygon; using autoware::mission_planner::lanelet2::convertBasicPoint3dToPose; using autoware::mission_planner::lanelet2::convertCenterlineToPoints; diff --git a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp index 42d92bd0c91da..f2f076869e2ff 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp @@ -14,6 +14,8 @@ #include "autoware/objects_of_interest_marker_interface/marker_utils.hpp" +#include + namespace autoware::objects_of_interest_marker_interface::marker_utils { using geometry_msgs::msg::Point; diff --git a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp index 253d2ec05c246..cbdb2542b97e7 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -18,6 +18,8 @@ #include #include +#include + namespace autoware::objects_of_interest_marker_interface { using autoware_perception_msgs::msg::Shape; diff --git a/planning/autoware_obstacle_cruise_planner/README.md b/planning/autoware_obstacle_cruise_planner/README.md index 1cb8128110e7c..42a569c07ddb2 100644 --- a/planning/autoware_obstacle_cruise_planner/README.md +++ b/planning/autoware_obstacle_cruise_planner/README.md @@ -23,12 +23,11 @@ The `autoware_obstacle_cruise_planner` package has following modules. ### Output topics -| Name | Type | Description | -| ------------------------------- | ---------------------------------------------- | ------------------------------------- | -| `~/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 | +| Name | Type | Description | +| ------------------------------- | ---------------------------------------------- | -------------------------------- | +| `~/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 | ## Design diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index e4fb1c6cfe36d..ef6ae1662dcee 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -54,7 +54,6 @@ class PlannerInterface slow_down_param_(SlowDownParam(node)), stop_param_(StopParam(node, longitudinal_info)) { - stop_reasons_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factors_pub_ = node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = @@ -146,7 +145,6 @@ class PlannerInterface stop_watch_; // Publishers - rclcpp::Publisher::SharedPtr stop_reasons_pub_; rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index 541a8281b2f1d..04badd2a956ef 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -30,8 +30,6 @@ #include "sensor_msgs/msg/point_cloud2.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_debug_msgs/msg/float64_stamped.hpp" -#include "tier4_planning_msgs/msg/stop_factor.hpp" -#include "tier4_planning_msgs/msg/stop_reason_array.hpp" #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" @@ -58,9 +56,6 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32Stamped; using tier4_debug_msgs::msg::Float64Stamped; -using tier4_planning_msgs::msg::StopFactor; -using tier4_planning_msgs::msg::StopReason; -using tier4_planning_msgs::msg::StopReasonArray; using tier4_planning_msgs::msg::StopSpeedExceeded; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; diff --git a/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml b/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml index aaf6db3e36613..189cd44379276 100644 --- a/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml +++ b/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml @@ -31,6 +31,5 @@ - diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index a6a029530b1a5..8275f2a1cd7d4 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -32,6 +32,13 @@ #include #include +#include +#include +#include +#include +#include +#include +#include namespace { diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 7bf0f67129a20..62edf82beec5a 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -25,6 +25,12 @@ #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" +#include +#include +#include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp index 8e31411bfb4af..39a35204cda8f 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp @@ -16,7 +16,9 @@ #include +#include #include +#include VelocityOptimizer::VelocityOptimizer( const double max_s_weight, const double max_v_weight, const double over_s_safety_weight, diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index da5d083d6ddcf..f8982ed3fec2a 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -23,6 +23,11 @@ #include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include +#include +#include +#include + using autoware::signal_processing::LowpassFilter1d; namespace diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 61946a8e4b37f..b08bb10ef69cf 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -25,8 +25,12 @@ #include #include +#include +#include #include #include +#include +#include namespace { @@ -39,54 +43,6 @@ StopSpeedExceeded createStopSpeedExceededMsg( return msg; } -tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( - const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose, - const StopObstacle & stop_obstacle) -{ - // create header - std_msgs::msg::Header header; - header.frame_id = "map"; - header.stamp = planner_data.current_time; - - // create stop factor - StopFactor stop_factor; - stop_factor.stop_pose = stop_pose; - geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point; - stop_factor_point.z = stop_pose.position.z; - stop_factor.dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( - planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position); - stop_factor.stop_factor_points.emplace_back(stop_factor_point); - - // create stop reason stamped - StopReason stop_reason_msg; - stop_reason_msg.reason = StopReason::OBSTACLE_STOP; - stop_reason_msg.stop_factors.emplace_back(stop_factor); - - // create stop reason array - StopReasonArray stop_reason_array; - stop_reason_array.header = header; - stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); - return stop_reason_array; -} - -StopReasonArray makeEmptyStopReasonArray(const rclcpp::Time & current_time) -{ - // create header - std_msgs::msg::Header header; - header.frame_id = "map"; - header.stamp = current_time; - - // create stop reason stamped - StopReason stop_reason_msg; - stop_reason_msg.reason = StopReason::OBSTACLE_STOP; - - // create stop reason array - StopReasonArray stop_reason_array; - stop_reason_array.header = header; - stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); - return stop_reason_array; -} - double calcMinimumDistanceToStop( const double initial_vel, const double max_acc, const double min_acc) { @@ -226,7 +182,6 @@ std::vector PlannerInterface::generateStopTrajectory( : std::abs(vehicle_info_.min_longitudinal_offset_m); if (stop_obstacles.empty()) { - stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time)); velocity_factors_pub_->publish( obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time)); // delete marker @@ -381,9 +336,6 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; - const auto stop_reasons_msg = - makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle); - stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose)); // Store stop reason debug data diff --git a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp index 639feec856a7e..4d6e6568067f4 100644 --- a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp @@ -18,6 +18,11 @@ #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include +#include +#include +#include + namespace { PointWithStamp calcNearestCollisionPoint( diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index 3bf6ee311ca13..dd4373c115fe0 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -17,6 +17,10 @@ #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" +#include +#include +#include + namespace obstacle_cruise_utils { namespace diff --git a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp index ef5f695756b7e..2235704a3a070 100644 --- a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp @@ -20,6 +20,7 @@ #include +#include #include using autoware::motion_planning::ObstacleCruisePlannerNode; diff --git a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_utils.cpp b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_utils.cpp index f7dbd60596507..b32508cc2c50e 100644 --- a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_utils.cpp @@ -21,6 +21,9 @@ #include +#include +#include + StopObstacle generate_stop_obstacle(uint8_t label, double dist) { const std::string uuid{}; diff --git a/planning/autoware_obstacle_stop_planner/README.md b/planning/autoware_obstacle_stop_planner/README.md index d3965192cd4c3..700cb657786ff 100644 --- a/planning/autoware_obstacle_stop_planner/README.md +++ b/planning/autoware_obstacle_stop_planner/README.md @@ -24,10 +24,9 @@ ### Output topics -| 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 | +| Name | Type | Description | +| -------------------- | ---------------------------------- | ------------------------- | +| `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | ### Common Parameter diff --git a/planning/autoware_obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml b/planning/autoware_obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml index db38370d36dfb..0b0fdc6e00459 100644 --- a/planning/autoware_obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml +++ b/planning/autoware_obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml @@ -22,7 +22,6 @@ - diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp index 75d25f3f8db80..93526518306af 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp @@ -18,6 +18,8 @@ #include #include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -47,7 +49,6 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( { virtual_wall_pub_ = node_->create_publisher("~/virtual_wall", 1); debug_viz_pub_ = node_->create_publisher("~/debug/marker", 1); - stop_reason_pub_ = node_->create_publisher("~/output/stop_reasons", 1); velocity_factor_pub_ = node_->create_publisher("/planning/velocity_factors/obstacle_stop", 1); pub_debug_values_ = @@ -188,8 +189,6 @@ void ObstacleStopPlannerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* publish stop reason for autoware api */ - const auto stop_reason_msg = makeStopReasonArray(); - stop_reason_pub_->publish(stop_reason_msg); const auto velocity_factor_msg = makeVelocityFactorArray(); velocity_factor_pub_->publish(velocity_factor_msg); @@ -493,33 +492,6 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() return msg; } -StopReasonArray ObstacleStopPlannerDebugNode::makeStopReasonArray() -{ - // create header - Header header; - header.frame_id = "map"; - header.stamp = node_->now(); - - // create stop reason stamped - StopReason stop_reason_msg; - stop_reason_msg.reason = StopReason::OBSTACLE_STOP; - StopFactor stop_factor; - - if (stop_pose_ptr_ != nullptr) { - stop_factor.stop_pose = *stop_pose_ptr_; - if (stop_obstacle_point_ptr_ != nullptr) { - stop_factor.stop_factor_points.emplace_back(*stop_obstacle_point_ptr_); - } - stop_reason_msg.stop_factors.emplace_back(stop_factor); - } - - // create stop reason array - StopReasonArray stop_reason_array; - stop_reason_array.header = header; - stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); - return stop_reason_array; -} - VelocityFactorArray ObstacleStopPlannerDebugNode::makeVelocityFactorArray() { VelocityFactorArray velocity_factor_array; diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp index 4400368cc3d70..d13e775e3d683 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -47,9 +46,6 @@ using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_planning_msgs::msg::StopFactor; -using tier4_planning_msgs::msg::StopReason; -using tier4_planning_msgs::msg::StopReasonArray; enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown, Obstacle }; @@ -113,7 +109,6 @@ class ObstacleStopPlannerDebugNode bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); MarkerArray makeVirtualWallMarker(); MarkerArray makeVisualizationMarker(); - StopReasonArray makeStopReasonArray(); VelocityFactorArray makeVelocityFactorArray(); void setDebugValues(const DebugValues::TYPE type, const double val) @@ -125,7 +120,6 @@ class ObstacleStopPlannerDebugNode private: rclcpp::Publisher::SharedPtr virtual_wall_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr stop_reason_pub_; rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr pub_debug_values_; rclcpp::Node * node_; diff --git a/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp b/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp index 28c5523f49620..5cdb48a9d6a70 100644 --- a/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp @@ -31,6 +31,9 @@ #include #include +#include +#include +#include namespace autoware::motion_planning { diff --git a/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp b/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp index 99a1ab46d2304..9f840586a1959 100644 --- a/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp +++ b/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp @@ -20,6 +20,7 @@ #include +#include #include using autoware::motion_planning::ObstacleStopPlannerNode; diff --git a/planning/autoware_path_optimizer/docs/mpt.md b/planning/autoware_path_optimizer/docs/mpt.md index 4b35fd0e36ab5..634542a13295f 100644 --- a/planning/autoware_path_optimizer/docs/mpt.md +++ b/planning/autoware_path_optimizer/docs/mpt.md @@ -360,9 +360,9 @@ $$ To realize collision-free trajectory planning, we have to formulate constraints that the vehicle is inside the road and also does not collide with obstacles in linear equations. For linearity, we implemented some methods to approximate the vehicle shape with a set of circles, that is reliable and easy to implement. -- 1. Bicycle Model -- 2. Uniform Circles -- 3. Fitting Uniform Circles +1. Bicycle Model +2. Uniform Circles +3. Fitting Uniform Circles ![vehicle_circles](../media/vehicle_circles.svg) diff --git a/planning/autoware_path_optimizer/launch/launch_visualization.launch.xml b/planning/autoware_path_optimizer/launch/launch_visualization.launch.xml index 38b3d83e94991..1a65357e52d53 100644 --- a/planning/autoware_path_optimizer/launch/launch_visualization.launch.xml +++ b/planning/autoware_path_optimizer/launch/launch_visualization.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/planning/autoware_path_optimizer/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp index 397c334eca5b7..cf994dadf38cc 100644 --- a/planning/autoware_path_optimizer/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -18,6 +18,9 @@ #include "visualization_msgs/msg/marker_array.hpp" +#include +#include + namespace autoware::path_optimizer { using autoware::universe_utils::appendMarkerArray; diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index df07f3f98957a..bff3b78b6ba7d 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -26,8 +26,12 @@ #include #include #include +#include #include +#include #include +#include +#include namespace autoware::path_optimizer { diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 62a4882f8a4c1..9d0345e304235 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -25,6 +25,9 @@ #include #include #include +#include +#include +#include namespace autoware::path_optimizer { @@ -222,7 +225,7 @@ void PathOptimizer::resetPreviousData() void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { - time_keeper_->start_track(__func__); + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); stop_watch_.tic(); // check if input path is valid @@ -277,8 +280,6 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) autoware::motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); - - time_keeper_->end_track(__func__); } bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const diff --git a/planning/autoware_path_optimizer/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp index 11e31bfd2d459..a52fbcc78211f 100644 --- a/planning/autoware_path_optimizer/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -19,6 +19,7 @@ #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/update_param.hpp" +#include #include namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 74033c5834db2..6bb95d665c8d5 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -16,6 +16,8 @@ #include "autoware/path_optimizer/mpt_optimizer.hpp" +#include + namespace autoware::path_optimizer { // state equation: x = B u + W (u includes x_0) diff --git a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index 9d24702cf75ea..e310fecf15d8c 100644 --- a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -20,6 +20,7 @@ #include +#include #include TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index 0bec4e46c8b42..20786b7b3601e 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -27,6 +27,9 @@ #include #include #include +#include +#include +#include namespace { @@ -34,9 +37,9 @@ Eigen::SparseMatrix makePMatrix(const int num_points) { std::vector> triplet_vec; const auto assign_value_to_triplet_vec = - [&](const double row, const double colum, const double value) { - triplet_vec.push_back(Eigen::Triplet(row, colum, value)); - triplet_vec.push_back(Eigen::Triplet(row + num_points, colum + num_points, value)); + [&](const double row, const double column, const double value) { + triplet_vec.push_back(Eigen::Triplet(row, column, value)); + triplet_vec.push_back(Eigen::Triplet(row + num_points, column + num_points, value)); }; for (int r = 0; r < num_points; ++r) { diff --git a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp index a3082a2c979c3..2569faa4b8232 100644 --- a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp +++ b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp @@ -22,6 +22,9 @@ #include #include +#include +#include +#include namespace autoware::path_smoother { diff --git a/planning/autoware_path_smoother/src/replan_checker.cpp b/planning/autoware_path_smoother/src/replan_checker.cpp index 5d2e7a05e4b17..5739aa7481831 100644 --- a/planning/autoware_path_smoother/src/replan_checker.cpp +++ b/planning/autoware_path_smoother/src/replan_checker.cpp @@ -19,6 +19,7 @@ #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/update_param.hpp" +#include #include namespace autoware::path_smoother diff --git a/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp index 7c92763f94521..7402541c727ff 100644 --- a/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp +++ b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp @@ -20,6 +20,7 @@ #include +#include #include TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp index 63ad2ecce09cc..b61c579df83b9 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include 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 1526e061ca4d1..62f89cab44e7b 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 @@ -19,6 +19,11 @@ #include #include +#include +#include +#include +#include + namespace autoware::planning_test_manager { @@ -279,9 +284,13 @@ void PlanningInterfaceTestManager::publishAbnormalRoute( void PlanningInterfaceTestManager::publishNominalPathWithLaneId( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, normal_path_with_lane_id_pub_, - autoware::test_utils::loadPathWithLaneIdInYaml(), 5); + try { + const auto path = autoware::test_utils::loadPathWithLaneIdInYaml(); + autoware::test_utils::publishToTargetNode( + test_node_, target_node, topic_name, normal_path_with_lane_id_pub_, path, 5); + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + } } void PlanningInterfaceTestManager::publishAbNominalPathWithLaneId( @@ -294,11 +303,14 @@ void PlanningInterfaceTestManager::publishAbNominalPathWithLaneId( void PlanningInterfaceTestManager::publishNominalPath( rclcpp::Node::SharedPtr target_node, std::string topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, normal_path_pub_, - autoware::motion_utils::convertToPath( - autoware::test_utils::loadPathWithLaneIdInYaml()), - 5); + try { + const auto path = autoware::test_utils::loadPathWithLaneIdInYaml(); + autoware::test_utils::publishToTargetNode( + test_node_, target_node, topic_name, normal_path_pub_, + autoware::motion_utils::convertToPath(path), 5); + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + } } void PlanningInterfaceTestManager::publishAbnormalPath( diff --git a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp index ff2f3084fc9ba..76bb660259323 100644 --- a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp @@ -17,6 +17,8 @@ #include #include +#include + namespace autoware::planning_topic_converter { namespace diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp index 0a70b4d415dcb..4304cd6146647 100644 --- a/planning/autoware_planning_validator/src/utils.cpp +++ b/planning/autoware_planning_validator/src/utils.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_functions.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_functions.cpp index 9282392197665..6b0370fd6cf23 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_functions.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_functions.cpp @@ -19,6 +19,7 @@ #include +#include #include using autoware::planning_validator::PlanningValidator; diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp index 6476086c67730..2cf03e04311f2 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -20,6 +20,7 @@ #include +#include #include using autoware::planning_test_manager::PlanningInterfaceTestManager; diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_pubsub.cpp index a83fa1fd965fe..882ff09cfa33a 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -22,7 +22,10 @@ #include +#include #include +#include +#include #include /* diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index ec5f7bb5e0080..01e52d85f01c3 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -35,9 +35,11 @@ #include #include +#include #include #include #include +#include #include #include #include diff --git a/planning/autoware_route_handler/test/test_route_handler.hpp b/planning/autoware_route_handler/test/test_route_handler.hpp index 1a9359dc8e17d..1b15b539e2001 100644 --- a/planning/autoware_route_handler/test/test_route_handler.hpp +++ b/planning/autoware_route_handler/test/test_route_handler.hpp @@ -31,6 +31,7 @@ #include +#include #include #include namespace autoware::route_handler::test @@ -45,7 +46,11 @@ class TestRouteHandler : public ::testing::Test TestRouteHandler() { set_route_handler("2km_test.osm"); - set_test_route(lane_change_right_test_route_filename); + try { + set_test_route(lane_change_right_test_route_filename); + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + } } TestRouteHandler(const TestRouteHandler &) = delete; @@ -68,7 +73,14 @@ class TestRouteHandler : public ::testing::Test { const auto rh_test_route = get_absolute_path_to_route(autoware_route_handler_dir, route_filename); - route_handler_->setRoute(autoware::test_utils::parse(rh_test_route)); + if ( + const auto route_opt = + autoware::test_utils::parse>(rh_test_route)) { + route_handler_->setRoute(*route_opt); + } else { + throw std::runtime_error( + "Failed to parse YAML file: " + rh_test_route + ". The file might be corrupted."); + } } lanelet::ConstLanelets get_current_lanes() diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 6b3a6c7406d39..47bc278c30564 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -14,6 +14,9 @@ #include "autoware/rtc_interface/rtc_interface.hpp" +#include +#include + namespace { using tier4_rtc_msgs::msg::Module; diff --git a/planning/autoware_rtc_interface/test/test_rtc_interface.cpp b/planning/autoware_rtc_interface/test/test_rtc_interface.cpp index 69efae3987677..d4b0195186282 100644 --- a/planning/autoware_rtc_interface/test/test_rtc_interface.cpp +++ b/planning/autoware_rtc_interface/test/test_rtc_interface.cpp @@ -22,6 +22,9 @@ #include +#include +#include + using tier4_rtc_msgs::msg::State; namespace autoware::rtc_interface diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index eb3303766205f..4bb64e27368d8 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -14,6 +14,7 @@ #include "autoware/scenario_selector/node.hpp" +#include #include #include @@ -65,6 +66,22 @@ bool isInLane( return dist_to_nearest_lanelet < margin; } +bool isAlongLane( + const std::shared_ptr & route_handler, + const geometry_msgs::msg::Pose & current_pose) +{ + lanelet::ConstLanelet closest_lanelet; + if (!route_handler->getClosestLaneletWithConstrainsWithinRoute( + current_pose, &closest_lanelet, 0.0, M_PI_4)) { + return false; + } + const lanelet::BasicPoint2d src_point(current_pose.position.x, current_pose.position.y); + const auto dist_to_centerline = + lanelet::geometry::distanceToCenterline2d(closest_lanelet, src_point); + static constexpr double margin = 1.0; + return dist_to_centerline < margin; +} + bool isInParkingLot( const std::shared_ptr & lanelet_map_ptr, const geometry_msgs::msg::Pose & current_pose) @@ -215,10 +232,9 @@ bool ScenarioSelectorNode::isSwitchToParking(const bool is_stopped) bool ScenarioSelectorNode::isSwitchToLaneDriving() { - const auto is_in_lane = - isInLane(route_handler_->getLaneletMapPtr(), current_pose_->pose.pose.position); + const auto is_along_lane = isAlongLane(route_handler_, current_pose_->pose.pose); - if (!isEmptyParkingTrajectory() || !is_in_lane) { + if (!isEmptyParkingTrajectory() || !is_along_lane) { empty_parking_trajectory_time_ = {}; return false; } diff --git a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp index 3d5da19cbabbc..ef9af48967a11 100644 --- a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp +++ b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::scenario_selector { 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 7d97caf4b375f..9ccc5a183be45 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 @@ -35,7 +35,7 @@ - + diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index fb0b821e87644..b9b1267a343db 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -17,6 +17,7 @@ autoware_behavior_path_planner_common autoware_geography_utils + autoware_global_parameter_loader autoware_interpolation autoware_lanelet2_extension autoware_map_loader @@ -33,10 +34,8 @@ autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - global_parameter_loader rclcpp rclcpp_components - tier4_map_msgs python3-flask-cors rosidl_default_runtime diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index cbfcac5789812..dc950f190652b 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -22,6 +22,7 @@ #include #include +#include namespace autoware::static_centerline_generator { 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 1f72dea1cd35f..51725fb8a3799 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 @@ -23,6 +23,7 @@ #include "utils.hpp" #include +#include namespace autoware::static_centerline_generator { diff --git a/planning/autoware_static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp index 5afc7e58ba52b..811d57c6036ef 100644 --- a/planning/autoware_static_centerline_generator/src/main.cpp +++ b/planning/autoware_static_centerline_generator/src/main.cpp @@ -14,6 +14,9 @@ #include "static_centerline_generator_node.hpp" +#include +#include + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); 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 398edc215da3e..7f0264d02434e 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 @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include 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 17abb3e446994..c591dcfc73bd8 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 @@ -24,10 +24,10 @@ #include "rclcpp/rclcpp.hpp" #include "type_alias.hpp" +#include "autoware_map_msgs/msg/map_projector_info.hpp" #include "std_msgs/msg/empty.hpp" #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" -#include "tier4_map_msgs/msg/map_projector_info.hpp" #include #include @@ -36,10 +36,10 @@ namespace autoware::static_centerline_generator { +using autoware_map_msgs::msg::MapProjectorInfo; using autoware_static_centerline_generator::srv::LoadMap; using autoware_static_centerline_generator::srv::PlanPath; using autoware_static_centerline_generator::srv::PlanRoute; -using tier4_map_msgs::msg::MapProjectorInfo; struct CenterlineWithRoute { diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp index c045a50fec0d7..f84fe79cec288 100644 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -23,7 +23,10 @@ #include #include +#include +#include #include +#include namespace autoware::static_centerline_generator { diff --git a/planning/autoware_surround_obstacle_checker/README.md b/planning/autoware_surround_obstacle_checker/README.md index 1f4bf77145624..fbe749f2eec08 100644 --- a/planning/autoware_surround_obstacle_checker/README.md +++ b/planning/autoware_surround_obstacle_checker/README.md @@ -34,8 +34,6 @@ else endif -:Publish stop reason; - stop @enduml ``` @@ -93,7 +91,6 @@ As mentioned in stop condition section, it prevents chattering by changing thres | `~/output/velocity_limit_clear_command` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | | `~/output/max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | Velocity limit command | | `~/output/no_start_reason` | `diagnostic_msgs::msg::DiagnosticStatus` | No start reason | -| `~/output/stop_reasons` | `tier4_planning_msgs::msg::StopReasonArray` | Stop reasons | | `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization | | `~/debug/footprint` | `geometry_msgs::msg::PolygonStamped` | Ego vehicle base footprint for visualization | | `~/debug/footprint_offset` | `geometry_msgs::msg::PolygonStamped` | Ego vehicle footprint with `surround_check_distance` offset for visualization | diff --git a/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml b/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml index 0ba7d29090916..c7e440f6eb3e1 100644 --- a/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml +++ b/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml @@ -10,7 +10,6 @@ - diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index 57d9cc8bc67a7..bbd1cac04ed89 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -17,6 +17,8 @@ #include #include #include + +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -75,7 +77,6 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( clock_(clock) { debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); - stop_reason_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factor_pub_ = node.create_publisher("/planning/velocity_factors/surround_obstacle", 1); vehicle_footprint_pub_ = node.create_publisher("~/debug/footprint", 1); @@ -142,8 +143,6 @@ void SurroundObstacleCheckerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* publish stop reason for autoware api */ - const auto stop_reason_msg = makeStopReasonArray(); - stop_reason_pub_->publish(stop_reason_msg); const auto velocity_factor_msg = makeVelocityFactorArray(); velocity_factor_pub_->publish(velocity_factor_msg); @@ -171,33 +170,6 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() return msg; } -StopReasonArray SurroundObstacleCheckerDebugNode::makeStopReasonArray() -{ - // create header - std_msgs::msg::Header header; - header.frame_id = "map"; - header.stamp = this->clock_->now(); - - // create stop reason stamped - StopReason stop_reason_msg; - stop_reason_msg.reason = StopReason::SURROUND_OBSTACLE_CHECK; - StopFactor stop_factor; - - if (stop_pose_ptr_ != nullptr) { - stop_factor.stop_pose = *stop_pose_ptr_; - if (stop_obstacle_point_ptr_ != nullptr) { - stop_factor.stop_factor_points.emplace_back(*stop_obstacle_point_ptr_); - } - stop_reason_msg.stop_factors.emplace_back(stop_factor); - } - - // create stop reason array - StopReasonArray stop_reason_array; - stop_reason_array.header = header; - stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); - return stop_reason_array; -} - VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() { VelocityFactorArray velocity_factor_array; diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index 17ec2631b2dad..b2c350c1b4698 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -39,9 +38,6 @@ using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using geometry_msgs::msg::PolygonStamped; -using tier4_planning_msgs::msg::StopFactor; -using tier4_planning_msgs::msg::StopReason; -using tier4_planning_msgs::msg::StopReasonArray; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -69,7 +65,6 @@ class SurroundObstacleCheckerDebugNode private: rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr stop_reason_pub_; rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_pub_; @@ -85,7 +80,6 @@ class SurroundObstacleCheckerDebugNode geometry_msgs::msg::Pose self_pose_; MarkerArray makeVisualizationMarker(); - StopReasonArray makeStopReasonArray(); VelocityFactorArray makeVelocityFactorArray(); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 70e81fa3b1d12..0adca312ca733 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -32,6 +32,7 @@ #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -57,34 +58,6 @@ using autoware::universe_utils::createPoint; using autoware::universe_utils::pose2transform; using autoware_perception_msgs::msg::ObjectClassification; -namespace -{ -std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) -{ - const std::string json_dumps_pose = - (boost::format( - R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % - pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % - pose.orientation.y % pose.orientation.z) - .str(); - return json_dumps_pose; -} - -diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string & no_start_reason, const geometry_msgs::msg::Pose & stop_pose) -{ - diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; - diagnostic_msgs::msg::KeyValue no_start_reason_diag_kv; - no_start_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - no_start_reason_diag.name = "no_start_reason"; - no_start_reason_diag.message = no_start_reason; - no_start_reason_diag_kv.key = "no_start_pose"; - no_start_reason_diag_kv.value = jsonDumpsPose(stop_pose); - no_start_reason_diag.values.push_back(no_start_reason_diag_kv); - return no_start_reason_diag; -} -} // namespace - SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options) : Node("surround_obstacle_checker_node", node_options) { @@ -104,8 +77,6 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); // Publishers - pub_stop_reason_ = - this->create_publisher("~/output/no_start_reason", 1); pub_clear_velocity_limit_ = this->create_publisher( "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local()); pub_velocity_limit_ = this->create_publisher( @@ -256,10 +227,8 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->pushObstaclePoint(nearest_obstacle.value().second, PointType::NoStart); } - diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; if (state_ == State::STOP) { debug_ptr_->pushPose(odometry_ptr_->pose.pose, PoseType::NoStart); - no_start_reason_diag = makeStopReasonDiag("obstacle", odometry_ptr_->pose.pose); } tier4_debug_msgs::msg::Float64Stamped processing_time_msg; @@ -267,7 +236,6 @@ void SurroundObstacleCheckerNode::onTimer() processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); - pub_stop_reason_->publish(no_start_reason_diag); debug_ptr_->publish(); } @@ -411,8 +379,8 @@ std::optional SurroundObstacleCheckerNode: auto SurroundObstacleCheckerNode::isStopRequired( const bool is_obstacle_found, const bool is_vehicle_stopped, const State & state, - const std::optional & last_obstacle_found_time, - const double time_threshold) const -> std::pair> + const std::optional & last_obstacle_found_time, const double time_threshold) const + -> std::pair> { if (!is_vehicle_stopped) { return std::make_pair(false, std::nullopt); diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index 480a937a4a909..f84ad3a8f5987 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -89,8 +89,8 @@ class SurroundObstacleCheckerNode : public rclcpp::Node auto isStopRequired( const bool is_obstacle_found, const bool is_vehicle_stopped, const State & state, - const std::optional & last_obstacle_found_time, - const double time_threshold) const -> std::pair>; + const std::optional & last_obstacle_found_time, const double time_threshold) const + -> std::pair>; // ros mutable tf2_ros::Buffer tf_buffer_{get_clock()}; @@ -104,7 +104,6 @@ class SurroundObstacleCheckerNode : public rclcpp::Node sub_pointcloud_{this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; autoware::universe_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ this, "~/input/objects"}; - rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; rclcpp::Publisher::SharedPtr pub_processing_time_; diff --git a/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md index f8a5f29e064f7..b19f556e59830 100644 --- a/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md +++ b/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md @@ -34,8 +34,6 @@ else endif -:Publish stop reason; - stop @enduml ``` @@ -93,7 +91,6 @@ Stop condition の項で述べたように、状態によって障害物判定 | `~/output/velocity_limit_clear_command` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | | `~/output/max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | Velocity limit command | | `~/output/no_start_reason` | `diagnostic_msgs::msg::DiagnosticStatus` | No start reason | -| `~/output/stop_reasons` | `tier4_planning_msgs::msg::StopReasonArray` | Stop reasons | | `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization | ## Parameters diff --git a/planning/autoware_surround_obstacle_checker/test/test_surround_obstacle_checker.cpp b/planning/autoware_surround_obstacle_checker/test/test_surround_obstacle_checker.cpp index c5fbb7958208d..3951532ed0df0 100644 --- a/planning/autoware_surround_obstacle_checker/test/test_surround_obstacle_checker.cpp +++ b/planning/autoware_surround_obstacle_checker/test/test_surround_obstacle_checker.cpp @@ -20,6 +20,9 @@ #include +#include +#include + namespace autoware::surround_obstacle_checker { auto generateTestTargetNode() -> std::shared_ptr @@ -48,8 +51,8 @@ class SurroundObstacleCheckerNodeTest : public ::testing::Test auto isStopRequired( const bool is_obstacle_found, const bool is_vehicle_stopped, const State & state, - const std::optional & last_obstacle_found_time, - const double time_threshold) const -> std::pair> + const std::optional & last_obstacle_found_time, const double time_threshold) const + -> std::pair> { return node_->isStopRequired( is_obstacle_found, is_vehicle_stopped, state, last_obstacle_found_time, time_threshold); diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 3f1c313e052da..069363d2f65e0 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -164,10 +164,17 @@ class VelocitySmootherNode : public rclcpp::Node bool plan_from_ego_speed_on_manual_mode = true; } node_param_{}; + struct AccelerationRequest + { + bool request{false}; + double max_acceleration{0.0}; + double max_jerk{0.0}; + }; struct ExternalVelocityLimit { double velocity{0.0}; // current external_velocity_limit double dist{0.0}; // distance to set external velocity limit + AccelerationRequest acceleration_request; std::string sender{""}; }; ExternalVelocityLimit diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp index 7206a64ea32eb..d51431383f88a 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -83,6 +83,8 @@ class SmootherBase double getMinJerk() const; void setWheelBase(const double wheel_base); + void setMaxAccel(const double max_acceleration); + void setMaxJerk(const double max_jerk); void setParam(const BaseParam & param); BaseParam getBaseParam() const; diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index a358a1b61c11b..1e0f6119ac21e 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include // clang-format on @@ -319,6 +320,7 @@ void VelocitySmootherNode::calcExternalVelocityLimit() autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!external_velocity_limit_ptr_) { + external_velocity_limit_.acceleration_request.request = false; return; } @@ -342,6 +344,20 @@ void VelocitySmootherNode::calcExternalVelocityLimit() external_velocity_limit_.dist = 0.0; } + const auto base_max_acceleration = get_parameter("normal.max_acc").as_double(); + const auto acceleration_request = + external_velocity_limit_ptr_->use_constraints && + base_max_acceleration < external_velocity_limit_ptr_->constraints.max_acceleration; + if ( + acceleration_request && + current_odometry_ptr_->twist.twist.linear.x < external_velocity_limit_ptr_->max_velocity) { + external_velocity_limit_.acceleration_request.request = true; + external_velocity_limit_.acceleration_request.max_acceleration = + external_velocity_limit_ptr_->constraints.max_acceleration; + external_velocity_limit_.acceleration_request.max_jerk = + external_velocity_limit_ptr_->constraints.max_jerk; + } + // calculate distance and maximum velocity // to decelerate to external velocity limit with jerk and acceleration // constraints. @@ -627,6 +643,18 @@ bool VelocitySmootherNode::smoothVelocity( clipped.insert( clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + // Set maximum acceleration before applying smoother. Depends on acceleration request from + // external velocity limit + const double smoother_max_acceleration = + external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_acceleration + : get_parameter("normal.max_acc").as_double(); + const double smoother_max_jerk = external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_jerk + : get_parameter("normal.max_jerk").as_double(); + smoother_->setMaxAccel(smoother_max_acceleration); + smoother_->setMaxJerk(smoother_max_jerk); + std::vector debug_trajectories; if (!smoother_->apply( initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories, @@ -791,12 +819,15 @@ std::pair VelocitySmootherNode::ca "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f", vehicle_speed, target_vel, node_param_.engage_velocity, engage_vel_thr, stop_dist); - Motion initial_motion = {node_param_.engage_velocity, node_param_.engage_acceleration}; + const double engage_acceleration = + external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_acceleration + : node_param_.engage_acceleration; + Motion initial_motion = {node_param_.engage_velocity, engage_acceleration}; return {initial_motion, InitializeType::ENGAGING}; - } else { - RCLCPP_DEBUG( - get_logger(), "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist); } + RCLCPP_DEBUG( + get_logger(), "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist); } else if (target_vel > 0.0) { RCLCPP_WARN_THROTTLE( get_logger(), *clock_, 3000, diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 213e70283f9b1..fcbcde84fe1ea 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -19,6 +19,7 @@ #include "autoware/velocity_smoother/trajectory_utils.hpp" #include +#include #include #include #include diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index b218a662de175..0f61171a3bb70 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -22,7 +22,9 @@ #include #include #include +#include #include +#include #include #include diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index 678c8e01bf67e..3280512376a58 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index 60345ff0fa6f4..eaad896562ccd 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index 46faf10fe4a62..e4271a9c387de 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -24,6 +24,8 @@ #include #include +#include +#include #include namespace autoware::velocity_smoother @@ -97,6 +99,16 @@ void SmootherBase::setWheelBase(const double wheel_base) base_param_.wheel_base = wheel_base; } +void SmootherBase::setMaxAccel(const double max_acceleration) +{ + base_param_.max_accel = max_acceleration; +} + +void SmootherBase::setMaxJerk(const double max_jerk) +{ + base_param_.max_jerk = max_jerk; +} + void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index d384c319ee361..b590af1b3ee0a 100644 --- a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -20,6 +20,7 @@ #include +#include #include using autoware::planning_test_manager::PlanningInterfaceTestManager; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 0e0b0fcf63933..3080ba1045b41 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -19,6 +19,7 @@ #include #include +#include namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 991b5c8af656d..7e4bc4d5ba6ac 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index cdefc7f82125a..f679efacb6a44 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -21,6 +21,8 @@ #include #include +#include +#include #include using autoware::behavior_path_planner::BehaviorPathPlannerNode; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 33d33119602f5..44b56f4ea0990 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -28,6 +28,7 @@ #include #include +#include #include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index c162170ae5eb0..dc7efebe88fba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -36,6 +36,8 @@ #include #include #include +#include +#include #include namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index fc0fac5ff7b9d..a18a2440dcb3a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include using autoware::behavior_path_planner::BehaviorPathPlannerNode; diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index c04914ed1c72b..45c997f364633 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -17,6 +17,8 @@ #include "autoware/behavior_path_lane_change_module/interface.hpp" #include "scene.hpp" +#include + namespace autoware::behavior_path_planner { using autoware::behavior_path_planner::LaneChangeInterface; diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 4a5fb562eb140..ad37a90605233 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -20,6 +20,8 @@ #include #include +#include +#include #include using autoware::behavior_path_planner::BehaviorPathPlannerNode; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt index d3f7f7a4015f0..8e945093659c1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt @@ -19,6 +19,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp src/goal_planner_module.cpp src/manager.cpp + src/decision_state.cpp ) if(BUILD_EXAMPLES) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp index af64dc5220010..40c2bd201abc7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp @@ -41,9 +41,17 @@ #include #include +#include #include #include +#include #include +#include +#include +#include +#include +#include +#include using namespace std::chrono_literals; // NOLINT diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 5d7fe103a9346..914da46e60180 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -135,6 +135,43 @@ struct PullOverContextData std::optional last_path_idx_increment_time; }; +bool isOnModifiedGoal( + const Pose & current_pose, const std::optional & modified_goal_opt, + const GoalPlannerParameters & parameters); + +bool hasPreviousModulePathShapeChanged( + const BehaviorModuleOutput & previous_module_output, + const BehaviorModuleOutput & last_previous_module_output); +bool hasDeviatedFromLastPreviousModulePath( + const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output); +bool hasDeviatedFromCurrentPreviousModulePath( + const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output); + +bool needPathUpdate( + const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now, + const std::optional & modified_goal, + const std::optional & selected_time, const GoalPlannerParameters & parameters); + +bool checkOccupancyGridCollision( + const PathWithLaneId & path, + const std::shared_ptr occupancy_grid_map); + +bool isStopped( + std::deque & odometry_buffer, + const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower); + +// Flag class for managing whether a certain callback is running in multi-threading +class ScopedFlag +{ +public: + explicit ScopedFlag(std::atomic & flag) : flag_(flag) { flag_.store(true); } + + ~ScopedFlag() { flag_.store(false); } + +private: + std::atomic & flag_; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -193,18 +230,6 @@ class GoalPlannerModule : public SceneModuleInterface void processOnExit() override; void updateData() override; - void updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & goal_planner_params); - - void updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & goal_planner_params); - - void updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & goal_planner_params); - void postProcess() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( @@ -257,18 +282,6 @@ class GoalPlannerModule : public SceneModuleInterface std::optional gp_planner_data_{std::nullopt}; std::mutex gp_planner_data_mutex_; - // Flag class for managing whether a certain callback is running in multi-threading - class ScopedFlag - { - public: - explicit ScopedFlag(std::atomic & flag) : flag_(flag) { flag_.store(true); } - - ~ScopedFlag() { flag_.store(false); } - - private: - std::atomic & flag_; - }; - /* * state transitions and plan function used in each state * @@ -359,11 +372,6 @@ class GoalPlannerModule : public SceneModuleInterface mutable GoalPlannerDebugData debug_data_; mutable PoseWithString debug_stop_pose_with_info_; - // collision check - bool checkOccupancyGridCollision( - const PathWithLaneId & path, - const std::shared_ptr occupancy_grid_map) const; - // goal seach GoalCandidates generateGoalCandidates() const; @@ -380,18 +388,8 @@ class GoalPlannerModule : public SceneModuleInterface double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; // status - bool isStopped(); - bool isStopped( - std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(const PullOverContextData & ctx_data); - bool isOnModifiedGoal( - const Pose & current_pose, const std::optional & modified_goal_opt, - const GoalPlannerParameters & parameters) const; double calcModuleRequestLength() const; - bool needPathUpdate( - const Pose & current_pose, const double path_update_duration, - const std::optional & modified_goal_opt, - const GoalPlannerParameters & parameters) const; bool isStuck( const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, const std::shared_ptr planner_data, @@ -443,16 +441,6 @@ class GoalPlannerModule : public SceneModuleInterface TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data); std::optional ignore_signal_{std::nullopt}; - bool hasPreviousModulePathShapeChanged( - const BehaviorModuleOutput & previous_module_output, - const BehaviorModuleOutput & last_previous_module_output) const; - bool hasDeviatedFromLastPreviousModulePath( - const std::shared_ptr planner_data, - const BehaviorModuleOutput & last_previous_module_output) const; - bool hasDeviatedFromCurrentPreviousModulePath( - const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) const; - // timer for generating pull over path candidates in a separate thread void onTimer(); void onFreespaceParkingTimer(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp new file mode 100644 index 0000000000000..1a4a6e5a25589 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -0,0 +1,190 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_goal_planner_module/util.hpp" + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ + +using autoware::motion_utils::calcSignedArcLength; + +void PathDecisionStateController::transit_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path, + std::vector & ego_polygons_expanded) +{ + const auto next_state = get_next_state( + found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, + planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, + pull_over_path, ego_polygons_expanded); + current_state_ = next_state; +} + +PathDecisionState PathDecisionStateController::get_next_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path_opt, + std::vector & ego_polygons_expanded) const +{ + auto next_state = current_state_; + + // update safety + if (!parameters.safety_check_params.enable_safety_check) { + next_state.is_stable_safe = true; + } else { + if (is_current_safe) { + if (!next_state.safe_start_time) { + next_state.safe_start_time = now; + next_state.is_stable_safe = false; + } else { + next_state.is_stable_safe = + ((now - next_state.safe_start_time.value()).seconds() > + parameters.safety_check_params.keep_unsafe_time); + } + } else { + next_state.safe_start_time = std::nullopt; + next_state.is_stable_safe = false; + } + } + + // Once this function returns true, it will continue to return true thereafter + if (next_state.state == PathDecisionState::DecisionKind::DECIDED) { + return next_state; + } + + // if path is not safe, not decided + if (!found_pull_over_path || !pull_over_path_opt) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + const auto & pull_over_path = pull_over_path_opt.value(); + const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; + // If it is dangerous against dynamic objects before approval, do not determine the path. + // This eliminates a unsafe path to be approved + if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " + "approval"); + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + const auto & current_path = pull_over_path.getCurrentPath(); + if (current_state_.state == PathDecisionState::DecisionKind::DECIDING) { + const double hysteresis_factor = 0.9; + + // check goal pose collision + if ( + modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( + modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, + planner_data, static_target_objects)) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; + return next_state; + } + + // check current parking path collision + const auto & parking_path = pull_over_path.parking_path(); + const auto & parking_path_curvatures = pull_over_path.parking_path_curvatures(); + const double margin = + parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; + if (goal_planner_utils::checkObjectsCollision( + parking_path, parking_path_curvatures, static_target_objects, dynamic_target_objects, + planner_data->parameters, margin, + /*extract_static_objects=*/false, parameters.maximum_deceleration, + parameters.object_recognition_collision_check_max_extra_stopping_margin, + ego_polygons_expanded, true)) { + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; + return next_state; + } + + if (enable_safety_check && !next_state.is_stable_safe) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + // if enough time has passed since deciding status starts, transition to DECIDED + constexpr double check_collision_duration = 1.0; + const double elapsed_time_from_deciding = + (now - current_state_.deciding_start_time.value()).seconds(); + if (elapsed_time_from_deciding > check_collision_duration) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); + next_state.state = PathDecisionState::DecisionKind::DECIDED; + next_state.deciding_start_time = std::nullopt; + return next_state; + } + + // if enough time has NOT passed since deciding status starts, keep DECIDING + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", + elapsed_time_from_deciding); + return next_state; + } + + // if ego is sufficiently close to the start of the nearest candidate path, the path is decided + const auto & current_pose = planner_data->self_odometry->pose.pose; + const size_t ego_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); + + const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( + current_path.points, pull_over_path.start_pose().position); + const double dist_to_parking_start_pose = calcSignedArcLength( + current_path.points, current_pose.position, ego_segment_idx, + pull_over_path.start_pose().position, start_pose_segment_idx); + if (dist_to_parking_start_pose > parameters.decide_path_distance) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + // if object recognition for path collision check is enabled, transition to DECIDING to check + // collision for a certain period of time. Otherwise, transition to DECIDED directly. + if (parameters.use_object_recognition) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " + "period of time"); + next_state.state = PathDecisionState::DecisionKind::DECIDING; + next_state.deciding_start_time = now; + return next_state; + } + return {PathDecisionState::DecisionKind::DECIDED, std::nullopt}; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index fed42be0753d7..01d06a4964c61 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -28,7 +28,6 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include @@ -38,10 +37,15 @@ #include #include +#include +#include #include +#include #include #include #include +#include +#include #include #include @@ -53,7 +57,6 @@ using autoware::motion_utils::insertDecelPoint; using autoware::universe_utils::calcDistance2d; using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::inverseTransformPose; using nav_msgs::msg::OccupancyGrid; namespace autoware::behavior_path_planner @@ -100,7 +103,10 @@ GoalPlannerModule::GoalPlannerModule( } if (pull_over_planners_.empty()) { - RCLCPP_ERROR(getLogger(), "Not found enabled planner"); + RCLCPP_WARN( + getLogger(), + "No enabled planner found. The vehicle will stop in the road lane without pull over. Please " + "check the parameters if this is the intended behavior."); } // set selected goal searcher @@ -145,9 +151,27 @@ GoalPlannerModule::GoalPlannerModule( // time_keeper_->add_reporter(&std::cerr); } -bool GoalPlannerModule::hasPreviousModulePathShapeChanged( +bool isOnModifiedGoal( + const Pose & current_pose, const GoalCandidate & modified_goal, + const GoalPlannerParameters & parameters) +{ + return calcDistance2d(current_pose, modified_goal.goal_pose) < parameters.th_arrived_distance; +} + +bool isOnModifiedGoal( + const Pose & current_pose, const std::optional & modified_goal_opt, + const GoalPlannerParameters & parameters) +{ + if (!modified_goal_opt) { + return false; + } + + return isOnModifiedGoal(current_pose, modified_goal_opt.value(), parameters); +} + +bool hasPreviousModulePathShapeChanged( const BehaviorModuleOutput & previous_module_output, - const BehaviorModuleOutput & last_previous_module_output) const + const BehaviorModuleOutput & last_previous_module_output) { // Calculate the lateral distance between each point of the current path and the nearest point of // the last path @@ -180,25 +204,69 @@ bool GoalPlannerModule::hasPreviousModulePathShapeChanged( return false; } -bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath( - const std::shared_ptr planner_data, - const BehaviorModuleOutput & last_previous_module_output) const +bool hasDeviatedFromLastPreviousModulePath( + const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output) { return std::abs(autoware::motion_utils::calcLateralOffset( last_previous_module_output.path.points, - planner_data->self_odometry->pose.pose.position)) > 0.3; + planner_data.self_odometry->pose.pose.position)) > 0.3; } -bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath( - const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) const +bool hasDeviatedFromCurrentPreviousModulePath( + const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output) { constexpr double LATERAL_DEVIATION_THRESH = 0.3; return std::abs(autoware::motion_utils::calcLateralOffset( - previous_module_output.path.points, planner_data->self_odometry->pose.pose.position)) > + previous_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > LATERAL_DEVIATION_THRESH; } +bool needPathUpdate( + const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now, + const std::optional & modified_goal, + const std::optional & selected_time, const GoalPlannerParameters & parameters) +{ + const bool has_enough_time_passed = + selected_time ? (now - selected_time.value()).seconds() > path_update_duration : true; + return !isOnModifiedGoal(current_pose, modified_goal, parameters) && has_enough_time_passed; +} + +bool checkOccupancyGridCollision( + const PathWithLaneId & path, + const std::shared_ptr occupancy_grid_map) +{ + if (!occupancy_grid_map) { + return false; + } + const bool check_out_of_range = false; + return occupancy_grid_map->hasObstacleOnPath(path, check_out_of_range); +} + +bool isStopped( + std::deque & odometry_buffer, + const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower) +{ + odometry_buffer.push_back(self_odometry); + // Delete old data in buffer_stuck_ + while (rclcpp::ok()) { + const auto time_diff = rclcpp::Time(odometry_buffer.back()->header.stamp) - + rclcpp::Time(odometry_buffer.front()->header.stamp); + if (time_diff.seconds() < duration_lower) { + break; + } + odometry_buffer.pop_front(); + } + bool is_stopped = true; + for (const auto & odometry : odometry_buffer) { + const double ego_vel = utils::l2Norm(odometry->twist.twist.linear); + if (ego_vel > duration_lower) { + is_stopped = false; + break; + } + } + return is_stopped; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { @@ -231,8 +299,8 @@ void GoalPlannerModule::onTimer() !local_planner_data || !current_status_opt || !previous_module_output_opt || !last_previous_module_output_opt || !occupancy_grid_map || !parameters_opt || !goal_candidates_opt) { - RCLCPP_ERROR( - getLogger(), + RCLCPP_INFO_THROTTLE( + getLogger(), *clock_, 5000, "failed to get valid " "local_planner_data/current_status/previous_module_output/occupancy_grid_map/parameters_opt " "in onTimer"); @@ -273,7 +341,7 @@ void GoalPlannerModule::onTimer() local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return false; } - if (hasDeviatedFromCurrentPreviousModulePath(local_planner_data, previous_module_output)) { + if (hasDeviatedFromCurrentPreviousModulePath(*local_planner_data, previous_module_output)) { RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); return false; } @@ -285,7 +353,7 @@ void GoalPlannerModule::onTimer() return true; } if ( - hasDeviatedFromLastPreviousModulePath(local_planner_data, last_previous_module_output) && + hasDeviatedFromLastPreviousModulePath(*local_planner_data, last_previous_module_output) && current_state != PathDecisionState::DecisionKind::DECIDED) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; @@ -397,8 +465,8 @@ void GoalPlannerModule::onFreespaceParkingTimer() if ( !local_planner_data || !current_status_opt || !occupancy_grid_map || !parameters_opt || !vehicle_footprint_opt || !goal_candidates_opt) { - RCLCPP_ERROR( - getLogger(), + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "failed to get valid planner_data/current_status/parameters in " "onFreespaceParkingTimer"); return; @@ -464,8 +532,8 @@ void GoalPlannerModule::onFreespaceParkingTimer() parameters) && is_new_costmap && needPathUpdate( - local_planner_data->self_odometry->pose.pose, path_update_duration, modified_goal_opt, - parameters)) { + local_planner_data->self_odometry->pose.pose, path_update_duration, clock_->now(), + modified_goal_opt, thread_safe_data_.get_last_path_update_time(), parameters)) { auto goal_searcher = std::make_shared(parameters, vehicle_footprint); planFreespacePath( @@ -474,30 +542,6 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } -void GoalPlannerModule::updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & goal_planner_params) -{ - ego_predicted_path_params = - std::make_shared(goal_planner_params->ego_predicted_path_params); -} - -void GoalPlannerModule::updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & goal_planner_params) -{ - safety_check_params = - std::make_shared(goal_planner_params->safety_check_params); -} - -void GoalPlannerModule::updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & goal_planner_params) -{ - objects_filtering_params = - std::make_shared(goal_planner_params->objects_filtering_params); -} - void GoalPlannerModule::updateData() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -640,9 +684,11 @@ void GoalPlannerModule::updateData() void GoalPlannerModule::initializeSafetyCheckParameters() { - updateEgoPredictedPathParams(ego_predicted_path_params_, parameters_); - updateSafetyCheckParams(safety_check_params_, parameters_); - updateObjectsFilteringParams(objects_filtering_params_, parameters_); + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); } void GoalPlannerModule::processOnExit() @@ -758,7 +804,9 @@ bool GoalPlannerModule::isExecutionReady() const // path_decision_controller.get_current_state() is valid if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { if (!path_decision_controller_.get_current_state().is_stable_safe) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + RCLCPP_INFO_THROTTLE( + getLogger(), *clock_, 5000, + "Path is not safe against dynamic objects, so the candidate path is not approved."); return false; } } @@ -830,7 +878,8 @@ bool GoalPlannerModule::planFreespacePath( bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & context_data) { // return only before starting free space parking - if (!isStopped()) { + if (!isStopped( + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time)) { return false; } @@ -901,7 +950,8 @@ BehaviorModuleOutput GoalPlannerModule::plan() if (utils::isAllowedGoalModification(planner_data_->route_handler)) { if (!context_data_) { - RCLCPP_ERROR(getLogger(), " [pull_over] plan() is called without valid context_data"); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, " [pull_over] plan() is called without valid context_data"); } else { const auto & context_data = context_data_.value(); return planPullOver(context_data); @@ -1186,7 +1236,7 @@ void GoalPlannerModule::setOutput( if (!context_data.pull_over_path_opt) { // situation : not safe against static objects use stop_path output.path = generateStopPath(context_data); - RCLCPP_WARN_THROTTLE( + RCLCPP_INFO_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); setDrivableAreaInfo(context_data, output); return; @@ -1200,7 +1250,7 @@ void GoalPlannerModule::setOutput( // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints output.path = generateFeasibleStopPath(pull_over_path.getCurrentPath()); - RCLCPP_WARN_THROTTLE( + RCLCPP_INFO_THROTTLE( getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path"); debug_stop_pose_with_info_.set(std::string("feasible stop after approval")); } else { @@ -1384,8 +1434,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( PathDecisionState::DecisionKind::NOT_DECIDED && !is_freespace && needPathUpdate( - planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, modified_goal_opt, - *parameters_)) { + planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, clock_->now(), + modified_goal_opt, thread_safe_data_.get_last_path_update_time(), *parameters_)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ @@ -1462,7 +1512,9 @@ void GoalPlannerModule::postProcess() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!context_data_) { - RCLCPP_ERROR(getLogger(), " [pull_over] postProcess() is called without valid context_data"); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + " [pull_over] postProcess() is called without valid context_data. use dummy context data."); } const auto context_data_dummy = PullOverContextData(true, PredictedObjects{}, PredictedObjects{}, std::nullopt, {}, {}); @@ -1500,8 +1552,10 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() if (utils::isAllowedGoalModification(planner_data_->route_handler)) { if (!context_data_) { - RCLCPP_ERROR( - getLogger(), " [pull_over] planWaitingApproval() is called without valid context_data"); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + " [pull_over] planWaitingApproval() is called without valid context_data. use fixed goal " + "planner"); } else { const auto & context_data = context_data_.value(); return planPullOverAsCandidate(context_data); @@ -1686,37 +1740,6 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId return stop_path; } -bool GoalPlannerModule::isStopped( - std::deque & odometry_buffer, const double time) -{ - const std::lock_guard lock(mutex_); - odometry_buffer.push_back(planner_data_->self_odometry); - // Delete old data in buffer - while (rclcpp::ok()) { - const auto time_diff = rclcpp::Time(odometry_buffer.back()->header.stamp) - - rclcpp::Time(odometry_buffer.front()->header.stamp); - if (time_diff.seconds() < time) { - break; - } - odometry_buffer.pop_front(); - } - bool is_stopped = true; - for (const auto & odometry : odometry_buffer) { - const double ego_vel = utils::l2Norm(odometry->twist.twist.linear); - if (ego_vel > parameters_->th_stopped_velocity) { - is_stopped = false; - break; - } - } - return is_stopped; -} - -bool GoalPlannerModule::isStopped() -{ - const std::lock_guard lock(mutex_); - return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); -} - bool GoalPlannerModule::isStuck( const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, const std::shared_ptr planner_data, @@ -1737,7 +1760,7 @@ bool GoalPlannerModule::isStuck( } constexpr double stuck_time = 5.0; - if (!isStopped(odometry_buffer_stuck_, stuck_time)) { + if (!isStopped(odometry_buffer_stuck_, planner_data->self_odometry, stuck_time)) { return false; } @@ -1777,7 +1800,8 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d return false; } - if (!isStopped()) { + if (!isStopped( + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time)) { return false; } @@ -1813,18 +1837,6 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d parameters_->th_arrived_distance; } -bool GoalPlannerModule::isOnModifiedGoal( - const Pose & current_pose, const std::optional & modified_goal_opt, - const GoalPlannerParameters & parameters) const -{ - if (!modified_goal_opt) { - return false; - } - - return calcDistance2d(current_pose, modified_goal_opt.value().goal_pose) < - parameters.th_arrived_distance; -} - TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1902,17 +1914,6 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & return new_signal; } -bool GoalPlannerModule::checkOccupancyGridCollision( - const PathWithLaneId & path, - const std::shared_ptr occupancy_grid_map) const -{ - if (!occupancy_grid_map) { - return false; - } - const bool check_out_of_range = false; - return occupancy_grid_map->hasObstacleOnPath(path, check_out_of_range); -} - bool GoalPlannerModule::hasEnoughDistance( const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path) const { @@ -2544,10 +2545,10 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) occupancy_grid_map_, *parameters_)) { marker.text += " stuck"; } - */ if (isStopped()) { marker.text += " stopped"; } + */ if (debug_data_.freespace_planner.is_planning) { marker.text += @@ -2574,24 +2575,6 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } } -bool GoalPlannerModule::needPathUpdate( - const Pose & current_pose, const double path_update_duration, - const std::optional & modified_goal_opt, - const GoalPlannerParameters & parameters) const -{ - return !isOnModifiedGoal(current_pose, modified_goal_opt, parameters) && - hasEnoughTimePassedSincePathUpdate(path_update_duration); -} - -bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const -{ - if (!thread_safe_data_.get_last_path_update_time()) { - return true; - } - - return (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > duration; -} - void GoalPlannerModule::GoalPlannerData::initializeOccupancyGridMap( const PlannerData & planner_data, const GoalPlannerParameters & parameters) { @@ -2636,166 +2619,4 @@ void GoalPlannerModule::GoalPlannerData::update( goal_candidates = goal_candidates_; } -void PathDecisionStateController::transit_state( - const bool found_pull_over_path, const rclcpp::Time & now, - const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, - const std::optional & pull_over_path, - std::vector & ego_polygons_expanded) -{ - const auto next_state = get_next_state( - found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, - planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, - pull_over_path, ego_polygons_expanded); - current_state_ = next_state; -} - -PathDecisionState PathDecisionStateController::get_next_state( - const bool found_pull_over_path, const rclcpp::Time & now, - const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, - const std::optional & pull_over_path_opt, - std::vector & ego_polygons_expanded) const -{ - auto next_state = current_state_; - - // update safety - if (!parameters.safety_check_params.enable_safety_check) { - next_state.is_stable_safe = true; - } else { - if (is_current_safe) { - if (!next_state.safe_start_time) { - next_state.safe_start_time = now; - next_state.is_stable_safe = false; - } else { - next_state.is_stable_safe = - ((now - next_state.safe_start_time.value()).seconds() > - parameters.safety_check_params.keep_unsafe_time); - } - } else { - next_state.safe_start_time = std::nullopt; - next_state.is_stable_safe = false; - } - } - - // Once this function returns true, it will continue to return true thereafter - if (next_state.state == PathDecisionState::DecisionKind::DECIDED) { - return next_state; - } - - // if path is not safe, not decided - if (!found_pull_over_path || !pull_over_path_opt) { - next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; - return next_state; - } - - const auto & pull_over_path = pull_over_path_opt.value(); - const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; - // If it is dangerous against dynamic objects before approval, do not determine the path. - // This eliminates a unsafe path to be approved - if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { - RCLCPP_DEBUG( - logger_, - "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " - "approval"); - next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; - return next_state; - } - - const auto & current_path = pull_over_path.getCurrentPath(); - if (current_state_.state == PathDecisionState::DecisionKind::DECIDING) { - const double hysteresis_factor = 0.9; - - // check goal pose collision - if ( - modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( - modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, - planner_data, static_target_objects)) { - RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); - next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; - next_state.deciding_start_time = std::nullopt; - return next_state; - } - - // check current parking path collision - const auto & parking_path = pull_over_path.parking_path(); - const auto & parking_path_curvatures = pull_over_path.parking_path_curvatures(); - const double margin = - parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; - if (goal_planner_utils::checkObjectsCollision( - parking_path, parking_path_curvatures, static_target_objects, dynamic_target_objects, - planner_data->parameters, margin, - /*extract_static_objects=*/false, parameters.maximum_deceleration, - parameters.object_recognition_collision_check_max_extra_stopping_margin, - ego_polygons_expanded, true)) { - RCLCPP_DEBUG( - logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); - next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; - next_state.deciding_start_time = std::nullopt; - return next_state; - } - - if (enable_safety_check && !next_state.is_stable_safe) { - RCLCPP_DEBUG( - logger_, - "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); - next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; - return next_state; - } - - // if enough time has passed since deciding status starts, transition to DECIDED - constexpr double check_collision_duration = 1.0; - const double elapsed_time_from_deciding = - (now - current_state_.deciding_start_time.value()).seconds(); - if (elapsed_time_from_deciding > check_collision_duration) { - RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); - next_state.state = PathDecisionState::DecisionKind::DECIDED; - next_state.deciding_start_time = std::nullopt; - return next_state; - } - - // if enough time has NOT passed since deciding status starts, keep DECIDING - RCLCPP_DEBUG( - logger_, "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", - elapsed_time_from_deciding); - return next_state; - } - - // if ego is sufficiently close to the start of the nearest candidate path, the path is decided - const auto & current_pose = planner_data->self_odometry->pose.pose; - const size_t ego_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); - - const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - current_path.points, pull_over_path.start_pose().position); - const double dist_to_parking_start_pose = calcSignedArcLength( - current_path.points, current_pose.position, ego_segment_idx, - pull_over_path.start_pose().position, start_pose_segment_idx); - if (dist_to_parking_start_pose > parameters.decide_path_distance) { - next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; - return next_state; - } - - // if object recognition for path collision check is enabled, transition to DECIDING to check - // collision for a certain period of time. Otherwise, transition to DECIDED directly. - if (parameters.use_object_recognition) { - RCLCPP_DEBUG( - logger_, - "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " - "period of time"); - next_state.state = PathDecisionState::DecisionKind::DECIDING; - next_state.deciding_start_time = now; - return next_state; - } - return {PathDecisionState::DecisionKind::DECIDED, std::nullopt}; -} - } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 5607729e1030d..66192a00a99fa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -29,7 +29,9 @@ #include +#include #include +#include #include namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 452090571ac45..b51d5df42c75b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -48,11 +48,12 @@ FreespacePullOver::FreespacePullOver( vehicle_info, parameters.vehicle_shape_margin); if (parameters.freespace_parking_algorithm == "astar") { planner_ = std::make_unique( - parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters); + parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters, + node.get_clock()); } else if (parameters.freespace_parking_algorithm == "rrtstar") { planner_ = std::make_unique( - parameters.freespace_parking_common_parameters, vehicle_shape, - parameters.rrt_star_parameters); + parameters.freespace_parking_common_parameters, vehicle_shape, parameters.rrt_star_parameters, + node.get_clock()); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp index 63610f5ac31f7..31d8afffbdabe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -14,6 +14,9 @@ #include +#include +#include + namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index da99c830c68bb..501502125d7ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -22,7 +22,10 @@ #include #include +#include +#include #include +#include #include namespace autoware::behavior_path_planner @@ -218,14 +221,23 @@ std::optional ShiftPullOver::generatePullOverPath( shifted_path.path.points.push_back(p); } + // combine road lanes and shoulder lanes to find closest lanelet id + const auto lanes = std::invoke([&]() -> lanelet::ConstLanelets { + auto lanes = road_lanes; + lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); + return lanes; // not copy the value (Return Value Optimization) + }); + // set goal pose with velocity 0 { PathPointWithLaneId p{}; p.point.longitudinal_velocity_mps = 0.0; p.point.pose = goal_pose; - p.lane_ids = shifted_path.path.points.back().lane_ids; - for (const auto & lane : pull_over_lanes) { - p.lane_ids.push_back(lane.id()); + lanelet::Lanelet goal_lanelet{}; + if (lanelet::utils::query::getClosestLanelet(lanes, goal_pose, &goal_lanelet)) { + p.lane_ids = {goal_lanelet.id()}; + } else { + p.lane_ids = shifted_path.path.points.back().lane_ids; } shifted_path.path.points.push_back(p); } @@ -234,24 +246,13 @@ std::optional ShiftPullOver::generatePullOverPath( for (size_t i = path_shifter.getShiftLines().front().start_idx; i < shifted_path.path.points.size() - 1; ++i) { auto & point = shifted_path.path.points.at(i); - // set velocity point.point.longitudinal_velocity_mps = std::min(point.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); - - // add target lanes to points after shift start - // add road lane_ids if not found - for (const auto id : shifted_path.path.points.back().lane_ids) { - if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) { - point.lane_ids.push_back(id); - } - } - // add shoulder lane_id if not found - for (const auto & lane : pull_over_lanes) { - if ( - std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) == - point.lane_ids.end()) { - point.lane_ids.push_back(lane.id()); - } + lanelet::Lanelet lanelet{}; + if (lanelet::utils::query::getClosestLanelet(lanes, point.point.pose, &lanelet)) { + point.lane_ids = {lanelet.id()}; // overwrite lane_ids + } else { + point.lane_ids = shifted_path.path.points.at(i - 1).lane_ids; } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 75cdb2d892510..8ca9d9ab45dc3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -33,7 +33,11 @@ #include #include +#include +#include #include +#include +#include #include namespace autoware::behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp index 64dbac4ff9adf..de231b78041b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp @@ -20,6 +20,10 @@ #include +#include +#include +#include + class TestUtilWithMap : public ::testing::Test { protected: diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt index 6ae84881477e8..d1b6bfed868f0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt @@ -28,4 +28,7 @@ if(BUILD_TESTING) target_include_directories(test_${PROJECT_NAME} PRIVATE src) endif() -ament_auto_package(INSTALL_TO_SHARE config) +ament_auto_package(INSTALL_TO_SHARE + config + test_data +) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 3b23dd8b4212b..86d8d1c0c1413 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -351,6 +351,71 @@ stop @enduml ``` +#### Delay Lane Change Check + +In certain situations, when there are stopped vehicles along the target lane ahead of Ego vehicle, to avoid getting stuck, it is desired to perform the lane change maneuver after the stopped vehicle. +To do so, all static objects ahead of ego along the target lane are checked in order from closest to furthest, if any object satisfies the following conditions, lane change will be delayed and candidate path will be rejected. + +1. The distance from object to terminal end is sufficient to perform lane change +2. The distance to object is less than the lane changing length +3. The distance from object to next object is sufficient to perform lane change + +If the parameter `check_only_parked_vehicle` is set to `true`, the check will only consider objects which are determined as parked. + +The following flow chart illustrates the delay lane change check. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #White + +start +if (Is target objects, candidate path, OR current lane path empty?) then (yes) + #LightPink:Return false; + stop +else (no) +endif + +:Start checking objects from closest to furthest; +repeat + if (Is distance from object to terminal sufficient) then (yes) + else (no) + #LightPink:Return false; + stop + endif + + if (Is distance to object less than lane changing length) then (yes) + else (no) + if (Is only check parked vehicles and vehicle is not parked) then (yes) + else (no) + if(Is last object OR distance to next object is sufficient) then (yes) + #LightGreen: Return true; + stop + else (no) + endif + endif + endif + repeat while (Is finished checking all objects) is (FALSE) + +#LightPink: Return false; +stop + +@enduml +``` + +The following figures demonstrate different situations under which will or will not be triggered: + +1. Delay lane change will be triggered as there is sufficient distance ahead. + ![delay lane change 1](./images/delay_lane_change_1.drawio.svg) +2. Delay lane change will NOT be triggered as there is no sufficient distance ahead + ![delay lane change 2](./images/delay_lane_change_2.drawio.svg) +3. Delay lane change will be triggered by fist NPC as there is sufficient distance ahead. + ![delay lane change 3](./images/delay_lane_change_3.drawio.svg) +4. Delay lane change will be triggered by second NPC as there is sufficient distance ahead + ![delay lane change 4](./images/delay_lane_change_4.drawio.svg) +5. Delay lane change will NOT be triggered as there is no sufficient distance ahead. + ![delay lane change 5](./images/delay_lane_change_5.drawio.svg) + #### Candidate Path's Safety check See [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) @@ -525,8 +590,6 @@ The ego vehicle may need to secure ample inter-vehicle distance ahead of the tar ![enable collision check at prepare phase](./images/lane_change-enable_collision_check_at_prepare_phase.png) -The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects. - #### If the lane is blocked and multiple lane changes When driving on the public road with other vehicles, there exist scenarios where lane changes cannot be executed. Suppose the candidate path is evaluated as unsafe, for example, due to incoming vehicles in the adjacent lane. In that case, the ego vehicle can't change lanes, and it is impossible to reach the goal. Therefore, the ego vehicle must stop earlier at a certain distance and wait for the adjacent lane to be evaluated as safe. The minimum stopping distance can be computed from shift length and minimum lane changing velocity. @@ -820,25 +883,22 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | Name | Unit | Type | Description | Default value | | :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | | `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | -| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | | `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 2.0 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 | -| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | -| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | +| `trajectory.max_prepare_duration` | [s] | double | The maximum preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `trajectory.min_prepare_duration` | [s] | double | The minimum preparation time for the ego vehicle to be ready to perform lane change. | 2.0 | +| `trajectory.lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `trajectory.minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `trajectory.lon_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 | +| `trajectory.lat_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | +| `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | +| `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | +| `trajectory.lane_changing_decel_factor` | [-] | double | longitudinal deceleration factor during lane changing phase | 0.5 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | -| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | -| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | -| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | -| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | -| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | +| `lateral_acceleration.min_values` | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | +| `lateral_acceleration.max_values` | [m/s2] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | ### Parameter to judge if lane change is completed @@ -865,6 +925,15 @@ The following parameters are used to judge lane change completion. | `stuck_detection.velocity` | [m/s] | double | Velocity threshold for ego vehicle stuck detection | 0.1 | | `stuck_detection.stop_time` | [s] | double | Stop time threshold for ego vehicle stuck detection | 3.0 | +### Delay Lane Change + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------ | ---- | ------ | ----------------------------------------------------------------------------------------------------- | ------------- | +| `delay_lane_change.enable` | [-] | bool | Flag to enable/disable lane change delay feature | true | +| `delay_lane_change.check_only_parked_vehicle` | [-] | bool | Flag to limit delay feature for only parked vehicles | false | +| `delay_lane_change.min_road_shoulder_width` | [m] | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5 | +| `delay_lane_change.th_parked_vehicle_shift_ratio` | [-] | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked | 0.6 | + ### Collision checks #### Target Objects @@ -891,14 +960,14 @@ The following parameters are used to judge lane change completion. | Name | Unit | Type | Description | Default value | | :------------------------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `enable_collision_check_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | -| `enable_collision_check_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | true | -| `enable_collision_check_for_prepare_phase.turns` | [-] | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase. | true | -| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | -| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | -| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | -| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | -| `safety_check.collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | +| `collision_check.enable_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | +| `collision_check.enable_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | true | +| `collision_check.enable_for_prepare_phase.turns` | [-] | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase. | true | +| `collision_check.check_current_lanes` | [-] | boolean | If true, the lane change module always checks objects in the current lanes for collision assessment. If false, it only checks objects in the current lanes when the ego vehicle is stuck. | false | +| `collision_check.check_other_lanes` | [-] | boolean | If true, the lane change module includes objects in other lanes when performing collision assessment. | false | +| `collision_check.use_all_predicted_paths` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +| `collision_check.prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | #### safety constraints during lane change path is computed @@ -910,7 +979,7 @@ The following parameters are used to judge lane change completion. | `safety_check.execution.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 1.0 | | `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.execution.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | #### safety constraints specifically for stopped or parked vehicles diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index a2a6fbfd880e4..bf892b3058a16 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -1,42 +1,40 @@ /**: ros__parameters: lane_change: - backward_lane_length: 200.0 #[m] - prepare_duration: 4.0 # [s] - + backward_lane_length: 200.0 backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] backward_length_from_intersection: 5.0 # [m] - lane_changing_lateral_jerk: 0.5 # [m/s3] - - minimum_lane_changing_velocity: 2.78 # [m/s] - prediction_time_resolution: 0.5 # [s] - longitudinal_acceleration_sampling_num: 5 - lateral_acceleration_sampling_num: 3 - - # side walk parked vehicle - object_check_min_road_shoulder_width: 0.5 # [m] - object_shiftable_ratio_threshold: 0.6 - # turn signal min_length_for_turn_signal_activation: 10.0 # [m] - length_ratio_for_turn_signal_deactivation: 0.8 # ratio (desired end position) - - # longitudinal acceleration - min_longitudinal_acc: -1.0 - max_longitudinal_acc: 1.0 - skip_process: - longitudinal_distance_diff_threshold: - prepare: 1.0 - lane_changing: 1.0 + # trajectory generation + trajectory: + max_prepare_duration: 4.0 + min_prepare_duration: 2.0 + lateral_jerk: 0.5 + min_longitudinal_acc: -1.0 + max_longitudinal_acc: 1.0 + th_prepare_length_diff: 1.0 + th_lane_changing_length_diff: 1.0 + min_lane_changing_velocity: 2.78 + lon_acc_sampling_num: 5 + lat_acc_sampling_num: 3 + lane_changing_decel_factor: 0.5 + + # delay lane change + delay_lane_change: + enable: true + check_only_parked_vehicle: false + min_road_shoulder_width: 0.5 # [m] + th_parked_vehicle_shift_ratio: 0.6 # safety check safety_check: allow_loose_check_for_cancel: true enable_target_lane_bound_check: true - collision_check_yaw_diff_threshold: 3.1416 + stopped_object_velocity_threshold: 1.0 # [m/s] execution: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 @@ -104,14 +102,16 @@ stop_time: 3.0 # [s] # collision check - enable_collision_check_for_prepare_phase: - general_lanes: false - intersection: true - turns: true - stopped_object_velocity_threshold: 1.0 # [m/s] - check_objects_on_current_lanes: false - check_objects_on_other_lanes: false - use_all_predicted_path: false + collision_check: + enable_for_prepare_phase: + general_lanes: false + intersection: true + turns: true + prediction_time_resolution: 0.5 + yaw_diff_threshold: 3.1416 + check_current_lanes: false + check_other_lanes: false + use_all_predicted_paths: false # lane change cancel cancel: diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg new file mode 100644 index 0000000000000..f5f3e4e88559b --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg @@ -0,0 +1,657 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg new file mode 100644 index 0000000000000..8237ac312aadb --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg @@ -0,0 +1,625 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg new file mode 100644 index 0000000000000..2bcbfb5cdb93d --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg @@ -0,0 +1,683 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg new file mode 100644 index 0000000000000..b38092183db14 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg @@ -0,0 +1,683 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg new file mode 100644 index 0000000000000..d4abb53a84999 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg @@ -0,0 +1,677 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index bd309dd35a260..07920f83fd980 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -93,6 +93,8 @@ class LaneChangeInterface : public SceneModuleInterface MarkerArray getModuleVirtualWall() override; protected: + using SceneModuleInterface::updateRTCStatus; + std::shared_ptr parameters_; std::unique_ptr module_type_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 550925c4a10fc..7202a1c6a9108 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -54,7 +54,7 @@ class NormalLaneChange : public LaneChangeBase void update_lanes(const bool is_approved) final; - void update_transient_data() final; + void update_transient_data(const bool is_approved) final; void update_filtered_objects() final; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index e5c7fcb6d621e..741b5afb50e08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -68,7 +68,7 @@ class LaneChangeBase virtual void update_lanes(const bool is_approved) = 0; - virtual void update_transient_data() = 0; + virtual void update_transient_data(const bool is_approved) = 0; virtual void update_filtered_objects() = 0; @@ -267,6 +267,15 @@ class LaneChangeBase return turn_signal; } + void set_signal_activation_time(const bool reset = false) const + { + if (reset) { + signal_activation_time_ = std::nullopt; + } else if (!signal_activation_time_) { + signal_activation_time_ = clock_.now(); + } + } + LaneChangeStatus status_{}; PathShifter path_shifter_{}; @@ -292,6 +301,7 @@ class LaneChangeBase mutable StopWatch stop_watch_; mutable lane_change::Debug lane_change_debug_; + mutable std::optional signal_activation_time_{std::nullopt}; rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index b6e6cb968ace8..29047c90590b3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -75,7 +75,7 @@ double calc_dist_to_last_fit_width( * of the maximum lane change preparation duration and the maximum velocity of the ego vehicle. * * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - `lc_param_ptr->lane_change_prepare_duration`: The duration allowed for lane change + * - `lc_param_ptr->maximum_prepare_duration`: The maximum duration allowed for lane change * preparation. * - `bpp_param_ptr->max_vel`: The maximum velocity of the ego vehicle. * @@ -106,8 +106,9 @@ double calc_ego_dist_to_lanes_start( const lanelet::ConstLanelets & target_lanes); double calc_lane_changing_acceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc); + const CommonDataPtr & common_data_ptr, const double initial_lane_changing_velocity, + const double max_path_velocity, const double lane_changing_time, + const double prepare_longitudinal_acc); /** * @brief Calculates the distance required during a lane change operation. @@ -130,6 +131,24 @@ std::vector calc_lon_acceleration_samples( const CommonDataPtr & common_data_ptr, const double max_path_velocity, const double prepare_duration); +/** + * @brief calculates the actual prepare duration that will be used for path generation + * + * @details computes the required prepare duration to be used for candidate path + * generation based on the elapsed time of turn signal activation, the minimum + * and maximum parameterized values for the prepare duration, + * and the minimum lane changing velocity. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which includes + * lane change parameters and bpp common parameters. + * @param current_velocity current ego vehicle velocity. + * @param active_signal_duration elapsed time since turn signal activation. + * @return The calculated prepare duration value in seconds (s) + */ +double calc_actual_prepare_duration( + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double active_signal_duration); + std::vector calc_prepare_phase_metrics( const CommonDataPtr & common_data_ptr, const double current_velocity, const double max_path_velocity, const double min_length_threshold = 0.0, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 231353e812dd1..c121ab8512cce 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -14,6 +14,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#include "autoware/behavior_path_lane_change_module/utils/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -42,140 +43,6 @@ using route_handler::RouteHandler; using universe_utils::Polygon2d; using utils::path_safety_checker::ExtendedPredictedObjects; -struct LateralAccelerationMap -{ - std::vector base_vel; - std::vector base_min_acc; - std::vector base_max_acc; - - void add(const double velocity, const double min_acc, const double max_acc) - { - if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { - return; - } - - size_t idx = 0; - for (size_t i = 0; i < base_vel.size(); ++i) { - if (velocity < base_vel.at(i)) { - break; - } - idx = i + 1; - } - - base_vel.insert(base_vel.begin() + idx, velocity); - base_min_acc.insert(base_min_acc.begin() + idx, min_acc); - base_max_acc.insert(base_max_acc.begin() + idx, max_acc); - } - - std::pair find(const double velocity) const - { - if (!base_vel.empty() && velocity < base_vel.front()) { - return std::make_pair(base_min_acc.front(), base_max_acc.front()); - } - if (!base_vel.empty() && velocity > base_vel.back()) { - return std::make_pair(base_min_acc.back(), base_max_acc.back()); - } - - const double min_acc = autoware::interpolation::lerp(base_vel, base_min_acc, velocity); - const double max_acc = autoware::interpolation::lerp(base_vel, base_max_acc, velocity); - - return std::make_pair(min_acc, max_acc); - } -}; - -struct CancelParameters -{ - bool enable_on_prepare_phase{true}; - bool enable_on_lane_changing_phase{false}; - double delta_time{1.0}; - double duration{5.0}; - double max_lateral_jerk{10.0}; - double overhang_tolerance{0.0}; - - // unsafe_hysteresis_threshold will be compare with the number of detected unsafe instance. If the - // number of unsafe exceeds unsafe_hysteresis_threshold, the lane change will be cancelled or - // aborted. - int unsafe_hysteresis_threshold{2}; - - int deceleration_sampling_num{5}; -}; - -struct Parameters -{ - // trajectory generation - double backward_lane_length{200.0}; - double prediction_time_resolution{0.5}; - int longitudinal_acc_sampling_num{10}; - int lateral_acc_sampling_num{10}; - - // lane change parameters - double backward_length_buffer_for_end_of_lane{0.0}; - double backward_length_buffer_for_blocking_object{0.0}; - double backward_length_from_intersection{5.0}; - double lane_changing_lateral_jerk{0.5}; - double minimum_lane_changing_velocity{5.6}; - double lane_change_prepare_duration{4.0}; - LateralAccelerationMap lane_change_lat_acc_map; - - // parked vehicle - double object_check_min_road_shoulder_width{0.5}; - double object_shiftable_ratio_threshold{0.6}; - - // turn signal - double min_length_for_turn_signal_activation{10.0}; - double length_ratio_for_turn_signal_deactivation{0.8}; - - // acceleration data - double min_longitudinal_acc{-1.0}; - double max_longitudinal_acc{1.0}; - - double skip_process_lon_diff_th_prepare{0.5}; - double skip_process_lon_diff_th_lane_changing{1.0}; - - // collision check - bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; - bool enable_collision_check_for_prepare_phase_in_intersection{true}; - bool enable_collision_check_for_prepare_phase_in_turns{true}; - double stopped_object_velocity_threshold{0.1}; - bool check_objects_on_current_lanes{true}; - bool check_objects_on_other_lanes{true}; - bool use_all_predicted_path{false}; - double lane_expansion_left_offset{0.0}; - double lane_expansion_right_offset{0.0}; - - // regulatory elements - bool regulate_on_crosswalk{false}; - bool regulate_on_intersection{false}; - bool regulate_on_traffic_light{false}; - - // ego vehicle stuck detection - double stop_velocity_threshold{0.1}; - double stop_time_threshold{3.0}; - - // true by default for all objects - utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; - - // safety check - bool allow_loose_check_for_cancel{true}; - bool enable_target_lane_bound_check{true}; - double collision_check_yaw_diff_threshold{3.1416}; - utils::path_safety_checker::RSSparams rss_params{}; - utils::path_safety_checker::RSSparams rss_params_for_parked{}; - utils::path_safety_checker::RSSparams rss_params_for_abort{}; - utils::path_safety_checker::RSSparams rss_params_for_stuck{}; - - // abort - CancelParameters cancel{}; - - // finish judge parameter - double lane_change_finish_judge_buffer{3.0}; - double finish_judge_lateral_threshold{0.2}; - double finish_judge_lateral_angle_deviation{autoware::universe_utils::deg2rad(3.0)}; - - // debug marker - bool publish_debug_marker{false}; -}; - enum class States { Normal = 0, Cancel, @@ -355,6 +222,8 @@ struct TransientData size_t current_path_seg_idx; // index of nearest segment to ego along current path double current_path_velocity; // velocity of the current path at the ego position along the path + double lane_change_prepare_duration{0.0}; + bool is_ego_near_current_terminal_start{false}; bool is_ego_stuck{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp new file mode 100644 index 0000000000000..a76742307e061 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp @@ -0,0 +1,180 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ + +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner::lane_change +{ +using utils::path_safety_checker::ObjectTypesToCheck; +using utils::path_safety_checker::RSSparams; + +struct LateralAccelerationMap +{ + std::vector base_vel; + std::vector base_min_acc; + std::vector base_max_acc; + + void add(const double velocity, const double min_acc, const double max_acc) + { + if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { + return; + } + + size_t idx = 0; + for (size_t i = 0; i < base_vel.size(); ++i) { + if (velocity < base_vel.at(i)) { + break; + } + idx = i + 1; + } + + base_vel.insert(base_vel.begin() + idx, velocity); + base_min_acc.insert(base_min_acc.begin() + idx, min_acc); + base_max_acc.insert(base_max_acc.begin() + idx, max_acc); + } + + std::pair find(const double velocity) const + { + if (!base_vel.empty() && velocity < base_vel.front()) { + return std::make_pair(base_min_acc.front(), base_max_acc.front()); + } + if (!base_vel.empty() && velocity > base_vel.back()) { + return std::make_pair(base_min_acc.back(), base_max_acc.back()); + } + + const double min_acc = autoware::interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = autoware::interpolation::lerp(base_vel, base_max_acc, velocity); + + return std::make_pair(min_acc, max_acc); + } +}; + +struct CancelParameters +{ + bool enable_on_prepare_phase{true}; + bool enable_on_lane_changing_phase{false}; + double delta_time{1.0}; + double duration{5.0}; + double max_lateral_jerk{10.0}; + double overhang_tolerance{0.0}; + + // th_unsafe_hysteresis will be compare with the number of detected unsafe instance. If the + // number of unsafe exceeds th_unsafe_hysteresis, the lane change will be cancelled or + // aborted. + int th_unsafe_hysteresis{2}; + + int deceleration_sampling_num{5}; +}; + +struct CollisionCheckParameters +{ + bool enable_for_prepare_phase_in_general_lanes{false}; + bool enable_for_prepare_phase_in_intersection{true}; + bool enable_for_prepare_phase_in_turns{true}; + bool check_current_lane{true}; + bool check_other_lanes{true}; + bool use_all_predicted_paths{false}; + double th_yaw_diff{3.1416}; + double prediction_time_resolution{0.5}; +}; + +struct SafetyParameters +{ + bool enable_loose_check_for_cancel{true}; + bool enable_target_lane_bound_check{true}; + double th_stopped_object_velocity{0.1}; + double lane_expansion_left_offset{0.0}; + double lane_expansion_right_offset{0.0}; + RSSparams rss_params{}; + RSSparams rss_params_for_parked{}; + RSSparams rss_params_for_abort{}; + RSSparams rss_params_for_stuck{}; + ObjectTypesToCheck target_object_types{}; + CollisionCheckParameters collision_check{}; +}; + +struct TrajectoryParameters +{ + double max_prepare_duration{4.0}; + double min_prepare_duration{1.0}; + double lateral_jerk{0.5}; + double min_longitudinal_acc{-1.0}; + double max_longitudinal_acc{1.0}; + double th_prepare_length_diff{0.5}; + double th_lane_changing_length_diff{0.5}; + double min_lane_changing_velocity{5.6}; + double lane_changing_decel_factor{0.5}; + int lon_acc_sampling_num{10}; + int lat_acc_sampling_num{10}; + LateralAccelerationMap lat_acc_map{}; +}; + +struct DelayParameters +{ + bool enable{true}; + bool check_only_parked_vehicle{false}; + double min_road_shoulder_width{0.5}; + double th_parked_vehicle_shift_ratio{0.6}; +}; + +struct Parameters +{ + TrajectoryParameters trajectory{}; + SafetyParameters safety{}; + CancelParameters cancel{}; + DelayParameters delay{}; + + // lane change parameters + double backward_lane_length{200.0}; + double backward_length_buffer_for_end_of_lane{0.0}; + double backward_length_buffer_for_blocking_object{0.0}; + double backward_length_from_intersection{5.0}; + + // parked vehicle + double object_check_min_road_shoulder_width{0.5}; + double th_object_shiftable_ratio{0.6}; + + // turn signal + double min_length_for_turn_signal_activation{10.0}; + + // regulatory elements + bool regulate_on_crosswalk{false}; + bool regulate_on_intersection{false}; + bool regulate_on_traffic_light{false}; + + // ego vehicle stuck detection + double th_stop_velocity{0.1}; + double th_stop_time{3.0}; + + // finish judge parameter + double lane_change_finish_judge_buffer{3.0}; + double th_finish_judge_lateral_diff{0.2}; + double th_finish_judge_yaw_diff{autoware::universe_utils::deg2rad(3.0)}; + + // debug marker + bool publish_debug_marker{false}; +}; + +} // namespace autoware::behavior_path_planner::lane_change + +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index a02a61d5e72b6..7c1c8e2c65abe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -116,10 +116,9 @@ CandidateOutput assignToCandidate( std::optional get_lane_change_target_lane( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes); -std::vector convertToPredictedPath( - const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, - const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters, const double resolution); +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const double lane_changing_acceleration); bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, @@ -132,14 +131,29 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); -bool passed_parked_objects( +/** + * @brief Checks if delaying of lane change maneuver is necessary + * + * @details Scans through the provided target objects (assumed to be ordered from closest to + * furthest), and returns true if any of the objects satisfy the following conditions: + * - Not near the end of current lanes + * - There is sufficient distance from object to next one to do lane change + * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects + * which pass isParkedObject() check will be considered. + * + * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, + * and transient data. + * @param lane_change_path Candidate lane change path to apply checks on. + * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include + * target lane leading static objects). + * @param object_debug Collision check debug struct to be updated if any of the target objects + * satisfy the conditions. + * @return bool True if conditions to delay lane change are met + */ +bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, CollisionCheckDebugMap & object_debug); - -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); + const std::vector & target_objects, + CollisionCheckDebugMap & object_debug); lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 44252bb9f2005..f80aad721a07c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -24,8 +24,10 @@ #include #include +#include #include #include +#include #include namespace autoware::behavior_path_planner @@ -78,7 +80,7 @@ void LaneChangeInterface::updateData() module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->update_filtered_objects(); - module_type_->update_transient_data(); + module_type_->update_transient_data(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { @@ -153,10 +155,10 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - *prev_approved_path_ = getPreviousModuleOutput().path; - BehaviorModuleOutput out = getPreviousModuleOutput(); + *prev_approved_path_ = out.path; + module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); @@ -166,7 +168,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); + path_reference_ = std::make_shared(out.reference_path); stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 10dba38645f39..1bb41069140e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -20,6 +20,7 @@ #include +#include #include #include #include @@ -43,49 +44,70 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s const auto parameter = [](std::string && name) { return "lane_change." + name; }; // trajectory generation - p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); - p.prediction_time_resolution = - getOrDeclareParameter(*node, parameter("prediction_time_resolution")); - p.longitudinal_acc_sampling_num = - getOrDeclareParameter(*node, parameter("longitudinal_acceleration_sampling_num")); - p.lateral_acc_sampling_num = - getOrDeclareParameter(*node, parameter("lateral_acceleration_sampling_num")); - - // parked vehicle detection - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, parameter("object_check_min_road_shoulder_width")); - p.object_shiftable_ratio_threshold = - getOrDeclareParameter(*node, parameter("object_shiftable_ratio_threshold")); + { + p.trajectory.max_prepare_duration = + getOrDeclareParameter(*node, parameter("trajectory.max_prepare_duration")); + p.trajectory.min_prepare_duration = + getOrDeclareParameter(*node, parameter("trajectory.min_prepare_duration")); + p.trajectory.lateral_jerk = + getOrDeclareParameter(*node, parameter("trajectory.lateral_jerk")); + p.trajectory.min_longitudinal_acc = + getOrDeclareParameter(*node, parameter("trajectory.min_longitudinal_acc")); + p.trajectory.max_longitudinal_acc = + getOrDeclareParameter(*node, parameter("trajectory.max_longitudinal_acc")); + p.trajectory.th_prepare_length_diff = + getOrDeclareParameter(*node, parameter("trajectory.th_prepare_length_diff")); + p.trajectory.th_lane_changing_length_diff = + getOrDeclareParameter(*node, parameter("trajectory.th_lane_changing_length_diff")); + p.trajectory.min_lane_changing_velocity = + getOrDeclareParameter(*node, parameter("trajectory.min_lane_changing_velocity")); + p.trajectory.lane_changing_decel_factor = + getOrDeclareParameter(*node, parameter("trajectory.lane_changing_decel_factor")); + p.trajectory.lon_acc_sampling_num = + getOrDeclareParameter(*node, parameter("trajectory.lon_acc_sampling_num")); + p.trajectory.lat_acc_sampling_num = + getOrDeclareParameter(*node, parameter("trajectory.lat_acc_sampling_num")); + + const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + p.trajectory.min_lane_changing_velocity = std::min( + p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.max_prepare_duration); + + // validation of trajectory parameters + if (p.trajectory.lon_acc_sampling_num < 1 || p.trajectory.lat_acc_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + node->get_logger().get_child(node_name), + "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " + << p.trajectory.lon_acc_sampling_num + << "Given lateral parameter: " << p.trajectory.lat_acc_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + + // lateral acceleration map + const auto lateral_acc_velocity = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); + const auto min_lateral_acc = getOrDeclareParameter>( + *node, parameter("lateral_acceleration.min_values")); + const auto max_lateral_acc = getOrDeclareParameter>( + *node, parameter("lateral_acceleration.max_values")); + if ( + lateral_acc_velocity.size() != min_lateral_acc.size() || + lateral_acc_velocity.size() != max_lateral_acc.size()) { + RCLCPP_ERROR( + node->get_logger().get_child(node_name), + "Lane change lateral acceleration map has invalid size."); + exit(EXIT_FAILURE); + } + for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { + p.trajectory.lat_acc_map.add( + lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); + } + } // turn signal p.min_length_for_turn_signal_activation = getOrDeclareParameter(*node, parameter("min_length_for_turn_signal_activation")); - p.length_ratio_for_turn_signal_deactivation = - getOrDeclareParameter(*node, parameter("length_ratio_for_turn_signal_deactivation")); - - // acceleration - p.min_longitudinal_acc = getOrDeclareParameter(*node, parameter("min_longitudinal_acc")); - p.max_longitudinal_acc = getOrDeclareParameter(*node, parameter("max_longitudinal_acc")); - - // collision check - p.enable_collision_check_for_prepare_phase_in_general_lanes = getOrDeclareParameter( - *node, parameter("enable_collision_check_for_prepare_phase.general_lanes")); - p.enable_collision_check_for_prepare_phase_in_intersection = getOrDeclareParameter( - *node, parameter("enable_collision_check_for_prepare_phase.intersection")); - p.enable_collision_check_for_prepare_phase_in_turns = - getOrDeclareParameter(*node, parameter("enable_collision_check_for_prepare_phase.turns")); - p.stopped_object_velocity_threshold = - getOrDeclareParameter(*node, parameter("stopped_object_velocity_threshold")); - p.check_objects_on_current_lanes = - getOrDeclareParameter(*node, parameter("check_objects_on_current_lanes")); - p.check_objects_on_other_lanes = - getOrDeclareParameter(*node, parameter("check_objects_on_other_lanes")); - p.use_all_predicted_path = - getOrDeclareParameter(*node, parameter("use_all_predicted_path")); - p.lane_expansion_left_offset = - getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); - p.lane_expansion_right_offset = - getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); + // lane change regulations p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = @@ -94,99 +116,85 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("regulation.traffic_light")); // ego vehicle stuck detection - p.stop_velocity_threshold = - getOrDeclareParameter(*node, parameter("stuck_detection.velocity")); - p.stop_time_threshold = - getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); - - // safety check - p.allow_loose_check_for_cancel = - getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); - p.enable_target_lane_bound_check = - getOrDeclareParameter(*node, parameter("safety_check.enable_target_lane_bound_check")); - p.collision_check_yaw_diff_threshold = getOrDeclareParameter( - *node, parameter("safety_check.collision_check_yaw_diff_threshold")); - - p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); - p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); - p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); - p.rss_params.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.execution.expected_front_deceleration")); - p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.execution.expected_rear_deceleration")); - p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.execution.rear_vehicle_reaction_time")); - p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin")); - p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( - *node, parameter("safety_check.execution.lateral_distance_max_threshold")); - - p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); - p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); - p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.parked.longitudinal_velocity_delta_time")); - p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.parked.expected_front_deceleration")); - p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.parked.expected_rear_deceleration")); - p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.parked.rear_vehicle_reaction_time")); - p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin")); - p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter( - *node, parameter("safety_check.parked.lateral_distance_max_threshold")); - - p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); - p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time")); - p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.cancel.expected_front_deceleration")); - p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.cancel.expected_rear_deceleration")); - p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.cancel.rear_vehicle_reaction_time")); - p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin")); - p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( - *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); - - p.rss_params_for_stuck.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.stuck.longitudinal_distance_min_threshold")); - p.rss_params_for_stuck.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.stuck.longitudinal_velocity_delta_time")); - p.rss_params_for_stuck.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.stuck.expected_front_deceleration")); - p.rss_params_for_stuck.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.stuck.expected_rear_deceleration")); - p.rss_params_for_stuck.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.stuck.rear_vehicle_reaction_time")); - p.rss_params_for_stuck.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.stuck.rear_vehicle_safety_time_margin")); - p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter( - *node, parameter("safety_check.stuck.lateral_distance_max_threshold")); + p.th_stop_velocity = getOrDeclareParameter(*node, parameter("stuck_detection.velocity")); + p.th_stop_time = getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + + // safety + { + p.safety.enable_loose_check_for_cancel = + getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.safety.enable_target_lane_bound_check = + getOrDeclareParameter(*node, parameter("safety_check.enable_target_lane_bound_check")); + p.safety.th_stopped_object_velocity = getOrDeclareParameter( + *node, parameter("safety_check.stopped_object_velocity_threshold")); + p.safety.lane_expansion_left_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); + p.safety.lane_expansion_right_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); + + // collision check + p.safety.collision_check.enable_for_prepare_phase_in_general_lanes = + getOrDeclareParameter( + *node, parameter("collision_check.enable_for_prepare_phase.general_lanes")); + p.safety.collision_check.enable_for_prepare_phase_in_intersection = getOrDeclareParameter( + *node, parameter("collision_check.enable_for_prepare_phase.intersection")); + p.safety.collision_check.enable_for_prepare_phase_in_turns = getOrDeclareParameter( + *node, parameter("collision_check.enable_for_prepare_phase.turns")); + p.safety.collision_check.check_current_lane = + getOrDeclareParameter(*node, parameter("collision_check.check_current_lanes")); + p.safety.collision_check.check_other_lanes = + getOrDeclareParameter(*node, parameter("collision_check.check_other_lanes")); + p.safety.collision_check.use_all_predicted_paths = + getOrDeclareParameter(*node, parameter("collision_check.use_all_predicted_paths")); + p.safety.collision_check.prediction_time_resolution = + getOrDeclareParameter(*node, parameter("collision_check.prediction_time_resolution")); + p.safety.collision_check.th_yaw_diff = + getOrDeclareParameter(*node, parameter("collision_check.yaw_diff_threshold")); + + // rss check + auto set_rss_params = [&](auto & params, const std::string & prefix) { + params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter(prefix + ".longitudinal_distance_min_threshold")); + params.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter(prefix + ".longitudinal_velocity_delta_time")); + params.front_vehicle_deceleration = + getOrDeclareParameter(*node, parameter(prefix + ".expected_front_deceleration")); + params.rear_vehicle_deceleration = + getOrDeclareParameter(*node, parameter(prefix + ".expected_rear_deceleration")); + params.rear_vehicle_reaction_time = + getOrDeclareParameter(*node, parameter(prefix + ".rear_vehicle_reaction_time")); + params.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter(prefix + ".rear_vehicle_safety_time_margin")); + params.lateral_distance_max_threshold = + getOrDeclareParameter(*node, parameter(prefix + ".lateral_distance_max_threshold")); + }; + set_rss_params(p.safety.rss_params, "safety_check.execution"); + set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked"); + set_rss_params(p.safety.rss_params_for_abort, "safety_check.cancel"); + set_rss_params(p.safety.rss_params_for_stuck, "safety_check.stuck"); + + // target object types + const std::string ns = "lane_change.target_object."; + p.safety.target_object_types.check_car = getOrDeclareParameter(*node, ns + "car"); + p.safety.target_object_types.check_truck = getOrDeclareParameter(*node, ns + "truck"); + p.safety.target_object_types.check_bus = getOrDeclareParameter(*node, ns + "bus"); + p.safety.target_object_types.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); + p.safety.target_object_types.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); + p.safety.target_object_types.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); + p.safety.target_object_types.check_motorcycle = + getOrDeclareParameter(*node, ns + "motorcycle"); + p.safety.target_object_types.check_pedestrian = + getOrDeclareParameter(*node, ns + "pedestrian"); + } // lane change parameters - const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); p.backward_length_buffer_for_end_of_lane = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); p.backward_length_buffer_for_blocking_object = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); p.backward_length_from_intersection = getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); - p.lane_changing_lateral_jerk = - getOrDeclareParameter(*node, parameter("lane_changing_lateral_jerk")); - p.lane_change_prepare_duration = - getOrDeclareParameter(*node, parameter("prepare_duration")); - p.minimum_lane_changing_velocity = - getOrDeclareParameter(*node, parameter("minimum_lane_changing_velocity")); - p.minimum_lane_changing_velocity = - std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( @@ -194,40 +202,14 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s "Lane change buffer must be more than 1 meter. Modifying the buffer."); } - // lateral acceleration map for lane change - const auto lateral_acc_velocity = - getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); - const auto min_lateral_acc = - getOrDeclareParameter>(*node, parameter("lateral_acceleration.min_values")); - const auto max_lateral_acc = - getOrDeclareParameter>(*node, parameter("lateral_acceleration.max_values")); - if ( - lateral_acc_velocity.size() != min_lateral_acc.size() || - lateral_acc_velocity.size() != max_lateral_acc.size()) { - RCLCPP_ERROR( - node->get_logger().get_child(node_name), - "Lane change lateral acceleration map has invalid size."); - exit(EXIT_FAILURE); - } - for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { - p.lane_change_lat_acc_map.add( - lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); - } - - // target object - { - const std::string ns = "lane_change.target_object."; - p.object_types_to_check.check_car = getOrDeclareParameter(*node, ns + "car"); - p.object_types_to_check.check_truck = getOrDeclareParameter(*node, ns + "truck"); - p.object_types_to_check.check_bus = getOrDeclareParameter(*node, ns + "bus"); - p.object_types_to_check.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); - p.object_types_to_check.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); - p.object_types_to_check.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); - p.object_types_to_check.check_motorcycle = - getOrDeclareParameter(*node, ns + "motorcycle"); - p.object_types_to_check.check_pedestrian = - getOrDeclareParameter(*node, ns + "pedestrian"); - } + // lane change delay + p.delay.enable = getOrDeclareParameter(*node, parameter("delay_lane_change.enable")); + p.delay.check_only_parked_vehicle = + getOrDeclareParameter(*node, parameter("delay_lane_change.check_only_parked_vehicle")); + p.delay.min_road_shoulder_width = + getOrDeclareParameter(*node, parameter("delay_lane_change.min_road_shoulder_width")); + p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( + *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); // lane change cancel p.cancel.enable_on_prepare_phase = @@ -240,7 +222,7 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("cancel.max_lateral_jerk")); p.cancel.overhang_tolerance = getOrDeclareParameter(*node, parameter("cancel.overhang_tolerance")); - p.cancel.unsafe_hysteresis_threshold = + p.cancel.th_unsafe_hysteresis = getOrDeclareParameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); p.cancel.deceleration_sampling_num = getOrDeclareParameter(*node, parameter("cancel.deceleration_sampling_num")); @@ -248,43 +230,35 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // finish judge parameters p.lane_change_finish_judge_buffer = getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); - p.finish_judge_lateral_threshold = + p.th_finish_judge_lateral_diff = getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); const auto finish_judge_lateral_angle_deviation = getOrDeclareParameter(*node, parameter("finish_judge_lateral_angle_deviation")); - p.finish_judge_lateral_angle_deviation = + p.th_finish_judge_yaw_diff = autoware::universe_utils::deg2rad(finish_judge_lateral_angle_deviation); // debug marker p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); - // validation of parameters - if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) { - RCLCPP_FATAL_STREAM( - node->get_logger().get_child(node_name), - "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " - << p.longitudinal_acc_sampling_num - << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } - // validation of safety check parameters - // if loosely check is not allowed, lane change module will keep on chattering and canceling, and + // if loose check is not enabled, lane change module will keep on chattering and canceling, and // false positive situation might occur - if (!p.allow_loose_check_for_cancel) { + if (!p.safety.enable_loose_check_for_cancel) { if ( - p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration || - p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration || - p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time || - p.rss_params.rear_vehicle_safety_time_margin > - p.rss_params_for_abort.rear_vehicle_safety_time_margin || - p.rss_params.lateral_distance_max_threshold > - p.rss_params_for_abort.lateral_distance_max_threshold || - p.rss_params.longitudinal_distance_min_threshold > - p.rss_params_for_abort.longitudinal_distance_min_threshold || - p.rss_params.longitudinal_velocity_delta_time > - p.rss_params_for_abort.longitudinal_velocity_delta_time) { + p.safety.rss_params.front_vehicle_deceleration > + p.safety.rss_params_for_abort.front_vehicle_deceleration || + p.safety.rss_params.rear_vehicle_deceleration > + p.safety.rss_params_for_abort.rear_vehicle_deceleration || + p.safety.rss_params.rear_vehicle_reaction_time > + p.safety.rss_params_for_abort.rear_vehicle_reaction_time || + p.safety.rss_params.rear_vehicle_safety_time_margin > + p.safety.rss_params_for_abort.rear_vehicle_safety_time_margin || + p.safety.rss_params.lateral_distance_max_threshold > + p.safety.rss_params_for_abort.lateral_distance_max_threshold || + p.safety.rss_params.longitudinal_distance_min_threshold > + p.safety.rss_params_for_abort.longitudinal_distance_min_threshold || + p.safety.rss_params.longitudinal_velocity_delta_time > + p.safety.rss_params_for_abort.longitudinal_velocity_delta_time) { RCLCPP_FATAL_STREAM( node->get_logger().get_child(node_name), "abort parameter might be loose... Terminating the program..."); @@ -323,8 +297,6 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "backward_lane_length", p->backward_lane_length); - updateParam(parameters, ns + "prepare_duration", p->lane_change_prepare_duration); - updateParam( parameters, ns + "backward_length_buffer_for_end_of_lane", p->backward_length_buffer_for_end_of_lane); @@ -335,59 +307,62 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorlane_change_finish_judge_buffer); updateParam( - parameters, ns + "lane_changing_lateral_jerk", p->lane_changing_lateral_jerk); + parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); + updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + } + { + const std::string ns = "lane_change.trajectory."; updateParam( - parameters, ns + "minimum_lane_changing_velocity", p->minimum_lane_changing_velocity); + parameters, ns + "max_prepare_duration", p->trajectory.max_prepare_duration); updateParam( - parameters, ns + "prediction_time_resolution", p->prediction_time_resolution); - + parameters, ns + "min_prepare_duration", p->trajectory.min_prepare_duration); + updateParam(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); + updateParam( + parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); + // longitudinal acceleration + updateParam( + parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc); + updateParam( + parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc); + updateParam( + parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor); int longitudinal_acc_sampling_num = 0; - updateParam( - parameters, ns + "longitudinal_acceleration_sampling_num", longitudinal_acc_sampling_num); + updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { - p->longitudinal_acc_sampling_num = longitudinal_acc_sampling_num; + p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num; } int lateral_acc_sampling_num = 0; - updateParam( - parameters, ns + "lateral_acceleration_sampling_num", lateral_acc_sampling_num); + updateParam(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); if (lateral_acc_sampling_num > 0) { - p->lateral_acc_sampling_num = lateral_acc_sampling_num; + p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num; } updateParam( - parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold); - updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); - - // longitudinal acceleration - updateParam(parameters, ns + "min_longitudinal_acc", p->min_longitudinal_acc); - updateParam(parameters, ns + "max_longitudinal_acc", p->max_longitudinal_acc); - } - - { - const std::string ns = "lane_change.skip_process.longitudinal_distance_diff_threshold."; - updateParam(parameters, ns + "prepare", p->skip_process_lon_diff_th_prepare); + parameters, ns + "th_prepare_length_diff", p->trajectory.th_prepare_length_diff); updateParam( - parameters, ns + "lane_changing", p->skip_process_lon_diff_th_lane_changing); + parameters, ns + "th_lane_changing_length_diff", p->trajectory.th_lane_changing_length_diff); } { const std::string ns = "lane_change.safety_check.lane_expansion."; - updateParam(parameters, ns + "left_offset", p->lane_expansion_left_offset); - updateParam(parameters, ns + "right_offset", p->lane_expansion_right_offset); + updateParam(parameters, ns + "left_offset", p->safety.lane_expansion_left_offset); + updateParam(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset); } { const std::string ns = "lane_change.target_object."; - updateParam(parameters, ns + "car", p->object_types_to_check.check_car); - updateParam(parameters, ns + "truck", p->object_types_to_check.check_truck); - updateParam(parameters, ns + "bus", p->object_types_to_check.check_bus); - updateParam(parameters, ns + "trailer", p->object_types_to_check.check_trailer); - updateParam(parameters, ns + "unknown", p->object_types_to_check.check_unknown); - updateParam(parameters, ns + "bicycle", p->object_types_to_check.check_bicycle); - updateParam(parameters, ns + "motorcycle", p->object_types_to_check.check_motorcycle); - updateParam(parameters, ns + "pedestrian", p->object_types_to_check.check_pedestrian); + updateParam(parameters, ns + "car", p->safety.target_object_types.check_car); + updateParam(parameters, ns + "truck", p->safety.target_object_types.check_truck); + updateParam(parameters, ns + "bus", p->safety.target_object_types.check_bus); + updateParam(parameters, ns + "trailer", p->safety.target_object_types.check_trailer); + updateParam(parameters, ns + "unknown", p->safety.target_object_types.check_unknown); + updateParam(parameters, ns + "bicycle", p->safety.target_object_types.check_bicycle); + updateParam( + parameters, ns + "motorcycle", p->safety.target_object_types.check_motorcycle); + updateParam( + parameters, ns + "pedestrian", p->safety.target_object_types.check_pedestrian); } { @@ -399,8 +374,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "velocity", p->stop_velocity_threshold); - updateParam(parameters, ns + "stop_time", p->stop_time_threshold); + updateParam(parameters, ns + "velocity", p->th_stop_velocity); + updateParam(parameters, ns + "stop_time", p->th_stop_time); } { @@ -425,7 +400,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); updateParam(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance); updateParam( - parameters, ns + "unsafe_hysteresis_threshold", p->cancel.unsafe_hysteresis_threshold); + parameters, ns + "unsafe_hysteresis_threshold", p->cancel.th_unsafe_hysteresis); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 942902b836c95..99e7cc73e59ff 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -130,7 +131,7 @@ void NormalLaneChange::update_lanes(const bool is_approved) *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } -void NormalLaneChange::update_transient_data() +void NormalLaneChange::update_transient_data(const bool is_approved) { if ( !common_data_ptr_ || !common_data_ptr_->is_data_available() || @@ -149,6 +150,13 @@ void NormalLaneChange::update_transient_data() prev_module_output_.path.points.at(nearest_seg_idx).point.longitudinal_velocity_mps; transient_data.current_path_seg_idx = nearest_seg_idx; + const auto active_signal_duration = + signal_activation_time_ ? (clock_.now() - signal_activation_time_.value()).seconds() : 0.0; + transient_data.lane_change_prepare_duration = + is_approved ? status_.lane_change_path.info.duration.prepare + : calculation::calc_actual_prepare_duration( + common_data_ptr_, common_data_ptr_->get_ego_speed(), active_signal_duration); + std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) = calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes()); @@ -289,7 +297,7 @@ bool NormalLaneChange::is_near_regulatory_element() const if (common_data_ptr_->transient_data.is_ego_near_current_terminal_start) return false; - const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; + const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; if (only_tl) { RCLCPP_DEBUG(logger_, "Stop time is over threshold. Ignore crosswalk and intersection checks."); @@ -327,6 +335,8 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const return get_terminal_turn_signal_info(); } + set_signal_activation_time(); + return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end); } @@ -355,9 +365,14 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; const auto current_nearest_seg_idx = common_data_ptr_->transient_data.current_path_seg_idx; - return getTurnSignalDecider().overwrite_turn_signal( + const auto turn_signal_info = getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); + + set_signal_activation_time( + turn_signal_info.turn_signal.command != terminal_turn_signal_info.turn_signal.command); + + return turn_signal_info; } LaneChangePath NormalLaneChange::getLaneChangePath() const @@ -428,8 +443,6 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path = utils::combinePath(output.path, *found_extended_path); } output.reference_path = getReferencePath(); - output.turn_signal_info = - get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); if (isStopState()) { const auto current_velocity = getEgoVelocity(); @@ -445,12 +458,17 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() extendOutputDrivableArea(output); + const auto turn_signal_info = + get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, - output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, + turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); + set_signal_activation_time( + output.turn_signal_info.turn_signal.command != turn_signal_info.turn_signal.command); + return output; } @@ -556,7 +574,8 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) // margin with leading vehicle // consider rss distance when the LC need to avoid obstacles const auto rss_dist = calcRssDistance( - 0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params_for_parked); + 0.0, lc_param_ptr->trajectory.min_lane_changing_velocity, + lc_param_ptr->safety.rss_params_for_parked); const auto stop_margin = transient_data.lane_changing_length.min + lc_param_ptr->backward_length_buffer_for_blocking_object + rss_dist + @@ -753,13 +772,13 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto yaw_deviation_to_centerline = utils::lane_change::calc_angle_to_lanelet_segment(target_lanes, current_pose); - if (yaw_deviation_to_centerline > lane_change_parameters_->finish_judge_lateral_angle_deviation) { + if (yaw_deviation_to_centerline > lane_change_parameters_->th_finish_judge_yaw_diff) { return false; } const auto & arc_length = common_data_ptr_->transient_data.target_lanes_ego_arc; const auto reach_target_lane = - std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; + std::abs(arc_length.distance) < lane_change_parameters_->th_finish_judge_lateral_diff; lane_change_debug_.distance_to_lane_change_finished = arc_length.distance; @@ -791,7 +810,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const planner_data_->parameters.ego_nearest_yaw_threshold); const double ego_velocity = - std::max(getEgoVelocity(), lane_change_parameters_->minimum_lane_changing_velocity); + std::max(getEgoVelocity(), lane_change_parameters_->trajectory.min_lane_changing_velocity); const double estimated_travel_dist = ego_velocity * lane_change_parameters_->cancel.delta_time; double dist = 0.0; @@ -952,12 +971,14 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects( insert_leading_objects(filtered_objects.target_lane_leading.stopped); insert_leading_objects(filtered_objects.target_lane_leading.stopped_at_bound); - const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; + const auto chk_obj_in_curr_lanes = + lane_change_parameters_->safety.collision_check.check_current_lane; if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { insert_leading_objects(filtered_objects.current_lane); } - const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes; + const auto chk_obj_in_other_lanes = + lane_change_parameters_->safety.collision_check.check_other_lanes; if (chk_obj_in_other_lanes) { insert_leading_objects(filtered_objects.others); } @@ -969,7 +990,7 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const { auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( - objects, lane_change_parameters_->object_types_to_check); + objects, lane_change_parameters_->safety.target_object_types); if (objects.objects.empty()) { return {}; @@ -1004,10 +1025,10 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const filtered_objects.target_lane_trailing.reserve(reserve_size); filtered_objects.others.reserve(reserve_size); - const auto stopped_obj_vel_th = common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; + const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; for (const auto & object : objects.objects) { - auto ext_object = utils::lane_change::transform(object, *common_data_ptr_->lc_param_ptr); + auto ext_object = utils::lane_change::transform(object, *lane_change_parameters_); const auto & ext_obj_pose = ext_object.initial_pose; ext_object.dist_from_ego = autoware::motion_utils::calcSignedArcLength( current_lanes_ref_path.points, current_pose.position, ext_obj_pose.position); @@ -1140,7 +1161,6 @@ std::vector NormalLaneChange::get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metric, const double shift_length, const double dist_to_reg_element) const { - const auto & route_handler = getRouteHandler(); const auto & transient_data = common_data_ptr_->transient_data; const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, @@ -1186,9 +1206,9 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto prepare_phase_metrics = get_prepare_metrics(); candidate_paths.reserve( - prepare_phase_metrics.size() * lane_change_parameters_->lateral_acc_sampling_num); + prepare_phase_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); - const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; + const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; const auto dist_to_next_regulatory_element = utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); @@ -1197,12 +1217,12 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) if (candidate_paths.empty()) return true; const auto prep_diff = std::abs(candidate_paths.back().info.length.prepare - prep_length); - if (prep_diff > lane_change_parameters_->skip_process_lon_diff_th_prepare) return true; + if (prep_diff > lane_change_parameters_->trajectory.th_prepare_length_diff) return true; if (!check_lc) return false; const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length); - return lc_diff > lane_change_parameters_->skip_process_lon_diff_th_lane_changing; + return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff; }; for (const auto & prep_metric : prepare_phase_metrics) { @@ -1338,7 +1358,7 @@ bool NormalLaneChange::check_candidate_path_safety( } if ( - !is_stuck && !utils::lane_change::passed_parked_objects( + !is_stuck && utils::lane_change::is_delay_lane_change( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped, lane_change_debug_.collision_check_objects)) { throw std::logic_error( @@ -1346,11 +1366,11 @@ bool NormalLaneChange::check_candidate_path_safety( } const auto lc_start_velocity = candidate_path.info.velocity.prepare; - const auto min_lc_velocity = lane_change_parameters_->minimum_lane_changing_velocity; + const auto min_lc_velocity = lane_change_parameters_->trajectory.min_lane_changing_velocity; constexpr double margin = 0.1; // path is unsafe if it exceeds target lane boundary with a high velocity if ( - lane_change_parameters_->enable_target_lane_bound_check && + lane_change_parameters_->safety.enable_target_lane_bound_check && lc_start_velocity > min_lc_velocity + margin && utils::lane_change::path_footprint_exceeds_target_lane_bound( common_data_ptr_, candidate_path.shifted_path.path, planner_data_->parameters.vehicle_info)) { @@ -1359,12 +1379,12 @@ bool NormalLaneChange::check_candidate_path_safety( constexpr size_t decel_sampling_num = 1; const auto safety_check_with_normal_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, decel_sampling_num, - lane_change_debug_.collision_check_objects); + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params, + decel_sampling_num, lane_change_debug_.collision_check_objects); if (!safety_check_with_normal_rss.is_safe && is_stuck) { const auto safety_check_with_stuck_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck, + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, decel_sampling_num, lane_change_debug_.collision_check_objects); return safety_check_with_stuck_rss.is_safe; } @@ -1392,7 +1412,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto & route_handler = *getRouteHandler(); const auto minimum_lane_changing_velocity = - lane_change_parameters_->minimum_lane_changing_velocity; + lane_change_parameters_->trajectory.min_lane_changing_velocity; const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); @@ -1433,10 +1453,10 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( target_lanes, lane_changing_start_pose.value()); const auto [min_lateral_acc, max_lateral_acc] = - lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); + lane_change_parameters_->trajectory.lat_acc_map.find(minimum_lane_changing_velocity); const auto lane_changing_time = autoware::motion_utils::calc_shift_time_from_jerk( - shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + shift_length, lane_change_parameters_->trajectory.lateral_jerk, max_lateral_acc); const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; const auto target_segment = getTargetSegment( @@ -1519,16 +1539,14 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {false, true}; } - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data); - - if (!has_passed_parked_objects) { + if (utils::lane_change::is_delay_lane_change( + common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data)) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); return {false, false}; } const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->rss_params_for_abort, + path, target_objects, lane_change_parameters_->safety.rss_params_for_abort, static_cast(lane_change_parameters_->cancel.deceleration_sampling_num), debug_data); { // only for debug purpose @@ -1562,7 +1580,7 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( } unsafe_hysteresis_count_ = 0; } - if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.unsafe_hysteresis_threshold) { + if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.th_unsafe_hysteresis) { RCLCPP_DEBUG( logger_, "%s: hysteresis count exceed threshold. lane change is now %s", __func__, (approved_path_safety_status.is_safe ? "safe" : "UNSAFE")); @@ -1624,7 +1642,7 @@ bool NormalLaneChange::calcAbortPath() const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); const auto current_velocity = - std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); + std::max(lane_change_parameters_->trajectory.min_lane_changing_velocity, getEgoVelocity()); const auto current_pose = getEgoPose(); const auto & selected_path = status_.lane_change_path; @@ -1707,7 +1725,7 @@ bool NormalLaneChange::calcAbortPath() shift_line.end_shift_length, abort_start_dist, current_velocity); path_shifter.setVelocity(current_velocity); const auto lateral_acc_range = - lane_change_parameters_->lane_change_lat_acc_map.find(current_velocity); + lane_change_parameters_->trajectory.lat_acc_map.find(current_velocity); const double & max_lateral_acc = lateral_acc_range.second; path_shifter.setLateralAccelerationLimit(max_lateral_acc); @@ -1809,8 +1827,6 @@ bool NormalLaneChange::has_collision_with_decel_patterns( return false; } - const auto current_pose = common_data_ptr_->get_ego_pose(); - const auto current_twist = common_data_ptr_->get_ego_twist(); const auto bpp_param = *common_data_ptr_->bpp_param_ptr; const auto global_min_acc = bpp_param.min_acc; const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; @@ -1827,21 +1843,16 @@ bool NormalLaneChange::has_collision_with_decel_patterns( acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(), [&](double n) { return lane_changing_acc + n * acc_resolution; }); - const auto time_resolution = lane_change_parameters_->prediction_time_resolution; - + const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; const auto all_collided = std::all_of( acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) { - const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( - lane_change_path, current_twist, current_pose, acceleration, bpp_param, - *lane_change_parameters_, time_resolution); - const auto debug_predicted_path = - utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); + const auto ego_predicted_path = utils::lane_change::convert_to_predicted_path( + common_data_ptr_, lane_change_path, acceleration); return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { - const auto selected_rss_param = - (obj.initial_twist.linear.x <= lane_change_parameters_->stopped_object_velocity_threshold) - ? lane_change_parameters_->rss_params_for_parked - : rss_param; + const auto selected_rss_param = (obj.initial_twist.linear.x <= stopped_obj_vel_th) + ? lane_change_parameters_->safety.rss_params_for_parked + : rss_param; return is_collided( lane_change_path, obj, ego_predicted_path, selected_rss_param, check_prepare_phase, debug_data); @@ -1880,11 +1891,11 @@ bool NormalLaneChange::is_collided( constexpr auto collision_check_yaw_diff_threshold{M_PI}; constexpr auto hysteresis_factor{1.0}; const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( - obj, lane_change_parameters_->use_all_predicted_path); + obj, lane_change_parameters_->safety.collision_check.use_all_predicted_paths); const auto safety_check_max_vel = get_max_velocity_for_safety_check(); const auto & bpp_param = *common_data_ptr_->bpp_param_ptr; - const double velocity_threshold = lane_change_parameters_->stopped_object_velocity_threshold; + const double velocity_threshold = lane_change_parameters_->safety.th_stopped_object_velocity; const double prepare_duration = lane_change_path.info.duration.prepare; const double start_time = check_prepare_phase ? 0.0 : prepare_duration; @@ -1946,13 +1957,13 @@ bool NormalLaneChange::is_ego_stuck() const universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->stop_velocity_threshold) { + if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->th_stop_velocity) { RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); return false; } // Ego is just stopped, not sure it is in stuck yet. - if (getStopTime() < lc_param_ptr->stop_time_threshold) { + if (getStopTime() < lc_param_ptr->th_stop_time) { RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); return false; } @@ -1960,8 +1971,8 @@ bool NormalLaneChange::is_ego_stuck() const // Check if any stationary object exist in obstacle_check_distance const auto & current_lanes_path = common_data_ptr_->current_lanes_path; const auto & ego_pose = common_data_ptr_->get_ego_pose(); - const auto rss_dist = - calcRssDistance(0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params); + const auto rss_dist = calcRssDistance( + 0.0, lc_param_ptr->trajectory.min_lane_changing_velocity, lc_param_ptr->safety.rss_params); // It is difficult to define the detection range. If it is too short, the stuck will not be // determined, even though you are stuck by an obstacle. If it is too long, @@ -1978,7 +1989,7 @@ bool NormalLaneChange::is_ego_stuck() const // Note: it needs chattering prevention. if ( std::abs(object.initial_twist.linear.x) > - lc_param_ptr->stopped_object_velocity_threshold) { // check if stationary + lc_param_ptr->safety.th_stopped_object_velocity) { // check if stationary return false; } @@ -2016,14 +2027,14 @@ void NormalLaneChange::updateStopTime() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto current_vel = getEgoVelocity(); - if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) { + if (std::abs(current_vel) > lane_change_parameters_->th_stop_velocity) { stop_time_ = 0.0; } else { const double duration = stop_watch_.toc("stop_time"); // clip stop time - if (stop_time_ + duration * 0.001 > lane_change_parameters_->stop_time_threshold) { + if (stop_time_ + duration * 0.001 > lane_change_parameters_->th_stop_time) { constexpr double eps = 0.1; - stop_time_ = lane_change_parameters_->stop_time_threshold + eps; + stop_time_ = lane_change_parameters_->th_stop_time + eps; } else { stop_time_ += duration * 0.001; } @@ -2037,7 +2048,7 @@ bool NormalLaneChange::check_prepare_phase() const const auto & route_handler = getRouteHandler(); const auto check_in_general_lanes = - lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes; + lane_change_parameters_->safety.collision_check.enable_for_prepare_phase_in_general_lanes; lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { @@ -2048,11 +2059,11 @@ bool NormalLaneChange::check_prepare_phase() const } const auto check_in_intersection = - lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection && + lane_change_parameters_->safety.collision_check.enable_for_prepare_phase_in_intersection && common_data_ptr_->transient_data.in_intersection; const auto check_in_turns = - lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns && + lane_change_parameters_->safety.collision_check.enable_for_prepare_phase_in_turns && common_data_ptr_->transient_data.in_turn_direction_lane; return check_in_intersection || check_in_turns || check_in_general_lanes; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 741a5d7b4761d..3ba11d62a2be7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -49,8 +49,8 @@ double calc_dist_from_pose_to_terminal_end( double calc_stopping_distance(const LCParamPtr & lc_param_ptr) { // v^2 = u^2 + 2ad - const auto min_lc_vel = lc_param_ptr->minimum_lane_changing_velocity; - const auto min_lon_acc = lc_param_ptr->min_longitudinal_acc; + const auto min_lc_vel = lc_param_ptr->trajectory.min_lane_changing_velocity; + const auto min_lon_acc = lc_param_ptr->trajectory.min_longitudinal_acc; const auto min_back_dist = std::abs((min_lc_vel * min_lc_vel) / (2 * min_lon_acc)); const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane; @@ -104,7 +104,7 @@ double calc_dist_to_last_fit_width( double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) { - const auto max_prepare_duration = common_data_ptr->lc_param_ptr->lane_change_prepare_duration; + const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.max_prepare_duration; const auto ego_max_speed = common_data_ptr->bpp_param_ptr->max_vel; return max_prepare_duration * ego_max_speed; @@ -146,7 +146,7 @@ double calc_minimum_acceleration( const double min_acc_threshold, const double prepare_duration) { if (prepare_duration < eps) return -std::abs(min_acc_threshold); - const double min_lc_velocity = lane_change_parameters.minimum_lane_changing_velocity; + const double min_lc_velocity = lane_change_parameters.trajectory.min_lane_changing_velocity; const double acc = (min_lc_velocity - current_velocity) / prepare_duration; return std::clamp(acc, -std::abs(min_acc_threshold), -eps); } @@ -167,10 +167,10 @@ std::vector calc_min_lane_change_lengths( return {}; } - const auto min_vel = lc_param_ptr->minimum_lane_changing_velocity; - const auto min_max_lat_acc = lc_param_ptr->lane_change_lat_acc_map.find(min_vel); + const auto min_vel = lc_param_ptr->trajectory.min_lane_changing_velocity; + const auto min_max_lat_acc = lc_param_ptr->trajectory.lat_acc_map.find(min_vel); const auto max_lat_acc = std::get<1>(min_max_lat_acc); - const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; + const auto lat_jerk = lc_param_ptr->trajectory.lateral_jerk; std::vector min_lc_lengths{}; min_lc_lengths.reserve(shift_intervals.size()); @@ -195,24 +195,23 @@ std::vector calc_max_lane_change_lengths( return {}; } - const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; - const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; - const auto t_prepare = lc_param_ptr->lane_change_prepare_duration; + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto lat_jerk = params.lateral_jerk; + const auto t_prepare = params.max_prepare_duration; const auto current_velocity = common_data_ptr->get_ego_speed(); const auto path_velocity = common_data_ptr->transient_data.current_path_velocity; const auto max_acc = calc_maximum_acceleration( - t_prepare, current_velocity, path_velocity, lc_param_ptr->max_longitudinal_acc); + t_prepare, current_velocity, path_velocity, params.max_longitudinal_acc); // TODO(Quda, Azu): should probably limit upper bound of velocity as well, but // disabled due failing evaluation tests. // const auto limit_vel = [&](const auto vel) { // const auto max_global_vel = common_data_ptr->bpp_param_ptr->max_vel; - // return std::clamp(vel, lc_param_ptr->minimum_lane_changing_velocity, max_global_vel); + // return std::clamp(vel, params.min_lane_changing_velocity, max_global_vel); // }; - auto vel = - std::max(common_data_ptr->get_ego_speed(), lc_param_ptr->minimum_lane_changing_velocity); + auto vel = std::max(common_data_ptr->get_ego_speed(), params.min_lane_changing_velocity); std::vector max_lc_lengths{}; @@ -222,7 +221,7 @@ std::vector calc_max_lane_change_lengths( vel = vel + max_acc * t_prepare; // lane changing section - const auto [min_lat_acc, max_lat_acc] = lc_param_ptr->lane_change_lat_acc_map.find(vel); + const auto [min_lat_acc, max_lat_acc] = params.lat_acc_map.find(vel); const auto t_lane_changing = autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); const auto lane_changing_length = @@ -307,8 +306,10 @@ std::pair calc_min_max_acceleration( const auto & bpp_params = *common_data_ptr->bpp_param_ptr; const auto current_ego_velocity = common_data_ptr->get_ego_speed(); - const auto min_accel_threshold = std::max(bpp_params.min_acc, lc_params.min_longitudinal_acc); - const auto max_accel_threshold = std::min(bpp_params.max_acc, lc_params.max_longitudinal_acc); + const auto min_accel_threshold = + std::max(bpp_params.min_acc, lc_params.trajectory.min_longitudinal_acc); + const auto max_accel_threshold = + std::min(bpp_params.max_acc, lc_params.trajectory.max_longitudinal_acc); // calculate minimum and maximum acceleration const auto min_acc = calc_minimum_acceleration( @@ -352,10 +353,12 @@ std::vector calc_lon_acceleration_samples( const auto & current_pose = common_data_ptr->get_ego_pose(); const auto & target_lanes = common_data_ptr->lanes_ptr->target; const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); - const auto sampling_num = common_data_ptr->lc_param_ptr->longitudinal_acc_sampling_num; + const auto sampling_num = common_data_ptr->lc_param_ptr->trajectory.lon_acc_sampling_num; - const auto [min_accel, max_accel] = + const auto min_max_accel = calc_min_max_acceleration(common_data_ptr, max_path_velocity, prepare_duration); + const auto & min_accel = min_max_accel.first; + const auto & max_accel = min_max_accel.second; const auto is_sampling_required = std::invoke([&]() -> bool { if (max_accel < 0.0 || transient_data.is_ego_stuck) return true; @@ -379,11 +382,17 @@ std::vector calc_lon_acceleration_samples( } double calc_lane_changing_acceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc) + const CommonDataPtr & common_data_ptr, const double initial_lane_changing_velocity, + const double max_path_velocity, const double lane_changing_time, + const double prepare_longitudinal_acc) { if (prepare_longitudinal_acc <= 0.0) { - return 0.0; + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto lane_changing_acc = + common_data_ptr->transient_data.is_ego_near_current_terminal_start + ? prepare_longitudinal_acc * params.lane_changing_decel_factor + : 0.0; + return lane_changing_acc; } return std::clamp( @@ -391,18 +400,41 @@ double calc_lane_changing_acceleration( prepare_longitudinal_acc); } +double calc_actual_prepare_duration( + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double active_signal_duration) +{ + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto min_lc_velocity = params.min_lane_changing_velocity; + + // need to ensure min prep duration is sufficient to reach minimum lane changing velocity + const auto min_prepare_duration = std::invoke([&]() -> double { + if (current_velocity >= min_lc_velocity) { + return params.min_prepare_duration; + } + const auto max_acc = + std::min(common_data_ptr->bpp_param_ptr->max_acc, params.max_longitudinal_acc); + if (max_acc < eps) { + return params.max_prepare_duration; + } + return (min_lc_velocity - current_velocity) / max_acc; + }); + + return std::max(params.max_prepare_duration - active_signal_duration, min_prepare_duration); +} + std::vector calc_prepare_durations(const CommonDataPtr & common_data_ptr) { const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; const auto threshold = common_data_ptr->bpp_param_ptr->base_link2front + lc_param_ptr->min_length_for_turn_signal_activation; - const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; // TODO(Azu) this check seems to cause scenario failures. if (common_data_ptr->transient_data.dist_to_terminal_start >= threshold) { - return {max_prepare_duration}; + return {common_data_ptr->transient_data.lane_change_prepare_duration}; } + const auto max_prepare_duration = lc_param_ptr->trajectory.max_prepare_duration; std::vector prepare_durations; constexpr double step = 0.5; @@ -418,7 +450,7 @@ std::vector calc_prepare_phase_metrics( const double max_path_velocity, const double min_length_threshold, const double max_length_threshold) { - const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity; const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; std::vector metrics; @@ -465,14 +497,15 @@ std::vector calc_shift_phase_metrics( const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, const double max_path_velocity, const double lon_accel, const double max_length_threshold) { - const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity; const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; // get lateral acceleration range const auto [min_lateral_acc, max_lateral_acc] = - common_data_ptr->lc_param_ptr->lane_change_lat_acc_map.find(initial_velocity); - const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / - common_data_ptr->lc_param_ptr->lateral_acc_sampling_num; + common_data_ptr->lc_param_ptr->trajectory.lat_acc_map.find(initial_velocity); + const auto lateral_acc_resolution = + std::abs(max_lateral_acc - min_lateral_acc) / + common_data_ptr->lc_param_ptr->trajectory.lat_acc_sampling_num; std::vector metrics; @@ -490,10 +523,10 @@ std::vector calc_shift_phase_metrics( for (double lat_acc = min_lateral_acc; lat_acc < max_lateral_acc + eps; lat_acc += lateral_acc_resolution) { const auto lane_changing_duration = autoware::motion_utils::calc_shift_time_from_jerk( - shift_length, common_data_ptr->lc_param_ptr->lane_changing_lateral_jerk, lat_acc); + shift_length, common_data_ptr->lc_param_ptr->trajectory.lateral_jerk, lat_acc); const double lane_changing_accel = calc_lane_changing_acceleration( - initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); + common_data_ptr, initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); const auto lane_changing_length = calculation::calc_phase_length( initial_velocity, max_vel, lane_changing_accel, lane_changing_duration); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 5700842780395..ff0bd4437c21c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 49551aa73f635..9ab6ac288dfd4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -599,52 +599,55 @@ std::optional get_lane_change_target_lane( return route_handler_ptr->getLaneChangeTargetExceptPreferredLane(current_lanes, direction); } -std::vector convertToPredictedPath( - const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters, const double resolution) +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const double lane_changing_acceleration) { if (lane_change_path.path.points.empty()) { return {}; } const auto & path = lane_change_path.path; - const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare; - const auto duration = lane_change_path.info.duration.sum(); - const auto prepare_time = lane_change_path.info.duration.prepare; - const auto & minimum_lane_changing_velocity = - lane_change_parameters.minimum_lane_changing_velocity; - + const auto & vehicle_pose = common_data_ptr->get_ego_pose(); + const auto & bpp_param_ptr = common_data_ptr->bpp_param_ptr; const auto nearest_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); + path.points, vehicle_pose, bpp_param_ptr->ego_nearest_dist_threshold, + bpp_param_ptr->ego_nearest_yaw_threshold); - std::vector predicted_path; const auto vehicle_pose_frenet = convertToFrenetPoint(path.points, vehicle_pose.position, nearest_seg_idx); - const double initial_velocity = std::abs(vehicle_twist.linear.x); + + const auto initial_velocity = common_data_ptr->get_ego_speed(); + const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare; + const auto duration = lane_change_path.info.duration.sum(); + const auto prepare_time = lane_change_path.info.duration.prepare; + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto resolution = lc_param_ptr->safety.collision_check.prediction_time_resolution; + std::vector predicted_path; // prepare segment for (double t = 0.0; t < prepare_time; t += resolution) { - const double velocity = - std::max(initial_velocity + prepare_acc * t, minimum_lane_changing_velocity); - const double length = initial_velocity * t + 0.5 * prepare_acc * t * t; + const auto velocity = + std::clamp(initial_velocity + prepare_acc * t, 0.0, lane_change_path.info.velocity.prepare); + const auto length = initial_velocity * t + 0.5 * prepare_acc * t * t; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } // lane changing segment - const double lane_changing_velocity = - std::max(initial_velocity + prepare_acc * prepare_time, minimum_lane_changing_velocity); - const double offset = + const auto lane_changing_velocity = std::clamp( + initial_velocity + prepare_acc * prepare_time, 0.0, lane_change_path.info.velocity.prepare); + const auto offset = initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time; for (double t = prepare_time; t < duration; t += resolution) { - const double delta_t = t - prepare_time; - const double velocity = lane_changing_velocity + lane_changing_acceleration * delta_t; - const double length = lane_changing_velocity * delta_t + - 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; + const auto delta_t = t - prepare_time; + const auto velocity = std::clamp( + lane_changing_velocity + lane_changing_acceleration * delta_t, 0.0, + lane_change_path.info.velocity.lane_changing); + const auto length = lane_changing_velocity * delta_t + + 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); @@ -668,9 +671,6 @@ bool isParkedObject( // +: object position // o: nearest point on centerline - using lanelet::geometry::distance2d; - using lanelet::geometry::toArcCoordinates; - const double object_vel_norm = std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y); if (object_vel_norm > static_object_velocity_threshold) { @@ -689,49 +689,15 @@ bool isParkedObject( const double lat_dist = autoware::motion_utils::calcLateralOffset(path.points, object_pose.position); - lanelet::BasicLineString2d bound; - double center_to_bound_buffer = 0.0; - if (lat_dist > 0.0) { - // left side vehicle - const auto most_left_road_lanelet = route_handler.getMostLeftLanelet(closest_lanelet); - const auto most_left_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute most_left_sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } - } - bound = most_left_lanelet.leftBound2d().basicLineString(); - if (most_left_sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } else { - // right side vehicle - const auto most_right_road_lanelet = route_handler.getMostRightLanelet(closest_lanelet); - const auto most_right_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages( - most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute most_right_sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } - } - bound = most_right_lanelet.rightBound2d().basicLineString(); - if (most_right_sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } + const auto most_side_lanelet = + lat_dist > 0.0 ? route_handler.getMostLeftLanelet(closest_lanelet, false, true) + : route_handler.getMostRightLanelet(closest_lanelet, false, true); + const auto bound = lat_dist > 0.0 ? most_side_lanelet.leftBound2d().basicLineString() + : most_side_lanelet.rightBound2d().basicLineString(); + const lanelet::Attribute lanelet_sub_type = + most_side_lanelet.attribute(lanelet::AttributeName::Subtype); + const auto center_to_bound_buffer = + lanelet_sub_type.value() == "road_shoulder" ? 0.0 : object_check_min_road_shoulder_width; return isParkedObject( closest_lanelet, bound, object, center_to_bound_buffer, object_shiftable_ratio_threshold); @@ -776,130 +742,60 @@ bool isParkedObject( return clamped_ratio > ratio_threshold; } -bool passed_parked_objects( +bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, CollisionCheckDebugMap & object_debug) + const ExtendedPredictedObjects & target_objects, CollisionCheckDebugMap & object_debug) { - const auto route_handler = *common_data_ptr->route_handler_ptr; - const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; - const auto & object_check_min_road_shoulder_width = - lane_change_parameters.object_check_min_road_shoulder_width; - const auto & object_shiftable_ratio_threshold = - lane_change_parameters.object_shiftable_ratio_threshold; const auto & current_lane_path = common_data_ptr->current_lanes_path; + const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay; - if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { - return true; - } - - const auto leading_obj_idx = getLeadingStaticObjectIdx( - route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold); - if (!leading_obj_idx) { - return true; - } - - const auto & leading_obj = objects.at(*leading_obj_idx); - auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); - const auto leading_obj_poly = - autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape); - if (leading_obj_poly.outer().empty()) { - return true; + if ( + !delay_lc_param.enable || target_objects.empty() || lane_change_path.path.points.empty() || + current_lane_path.points.empty()) { + return false; } - const auto & current_path_end = current_lane_path.points.back().point.pose.position; - const auto dist_to_path_end = [&](const auto & src_point) { - if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) { - const auto goal_pose = route_handler.getGoalPose(); - return motion_utils::calcSignedArcLength( - current_lane_path.points, src_point, goal_pose.position); - } - return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end); + const auto dist_to_end = common_data_ptr->transient_data.dist_to_terminal_end; + const auto dist_buffer = common_data_ptr->transient_data.current_dist_buffer.min; + auto is_near_end = [&dist_to_end, &dist_buffer](const ExtendedPredictedObject & obj) { + const auto dist_obj_to_end = dist_to_end - obj.dist_from_ego; + return dist_obj_to_end <= dist_buffer; }; - const auto min_dist_to_end_of_current_lane = std::invoke([&]() { - auto min_dist_to_end_of_current_lane = std::numeric_limits::max(); - for (const auto & point : leading_obj_poly.outer()) { - const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0); - const auto dist = dist_to_path_end(obj_p); - min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); - } - return min_dist_to_end_of_current_lane; - }); - - // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) { - return true; - } - - const auto current_pose = common_data_ptr->get_ego_pose(); - const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( - current_lane_path.points, current_pose.position, leading_obj.initial_pose.position); - - if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { - return true; - } - - debug.second.unsafe_reason = "delay lane change"; - utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return false; -} - -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold) -{ - const auto & path = lane_change_path.path; - - if (path.points.empty() || objects.empty()) { - return {}; - } - - const auto & lane_change_start = lane_change_path.info.lane_changing_start; - const auto & path_end = path.points.back(); - - double dist_lc_start_to_leading_obj = 0.0; - std::optional leading_obj_idx = std::nullopt; - for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { - const auto & obj = objects.at(obj_idx); - const auto & obj_pose = obj.initial_pose; - - // ignore non-static object - // TODO(shimizu): parametrize threshold - const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); - if (obj_vel_norm > 1.0) { - continue; - } + const auto ego_vel = common_data_ptr->get_ego_speed(); + const auto min_lon_acc = common_data_ptr->lc_param_ptr->trajectory.min_longitudinal_acc; + const auto gap_threshold = std::abs((ego_vel * ego_vel) / (2 * min_lon_acc)); + auto is_sufficient_gap = [&gap_threshold](const auto & current_obj, const auto & next_obj) { + const auto curr_obj_half_length = current_obj.shape.dimensions.x; + const auto next_obj_half_length = next_obj.shape.dimensions.x; + const auto dist_current_to_next = next_obj.dist_from_ego - current_obj.dist_from_ego; + const auto gap_length = dist_current_to_next - curr_obj_half_length - next_obj_half_length; + return gap_length > gap_threshold; + }; - // ignore non-parked object - if (!isParkedObject( - path, route_handler, obj, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold)) { - continue; - } + for (auto it = target_objects.begin(); it < target_objects.end(); ++it) { + if (is_near_end(*it)) break; - const double dist_back_to_obj = autoware::motion_utils::calcSignedArcLength( - path.points, path_end.point.pose.position, obj_pose.position); - if (dist_back_to_obj > 0.0) { - // object is not on the lane change path - continue; - } + if (it->dist_from_ego < lane_change_path.info.length.lane_changing) continue; - const double dist_lc_start_to_obj = autoware::motion_utils::calcSignedArcLength( - path.points, lane_change_start.position, obj_pose.position); - if (dist_lc_start_to_obj < 0.0) { - // object is on the lane changing path or behind it. It will be detected in safety check + if ( + delay_lc_param.check_only_parked_vehicle && + !isParkedObject( + lane_change_path.path, *common_data_ptr->route_handler_ptr, *it, + delay_lc_param.min_road_shoulder_width, delay_lc_param.th_parked_vehicle_shift_ratio)) { continue; } - if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) { - dist_lc_start_to_leading_obj = dist_lc_start_to_obj; - leading_obj_idx = obj_idx; + auto next_it = std::next(it); + if (next_it == target_objects.end() || is_sufficient_gap(*it, *next_it)) { + auto debug = utils::path_safety_checker::createObjectDebug(*it); + debug.second.unsafe_reason = "delay lane change"; + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); + return true; } } - return leading_obj_idx; + return false; } lanelet::BasicPolygon2d create_polygon( @@ -918,7 +814,8 @@ ExtendedPredictedObject transform( { ExtendedPredictedObject extended_object(object); - const auto & time_resolution = lane_change_parameters.prediction_time_resolution; + const auto & time_resolution = + lane_change_parameters.safety.collision_check.prediction_time_resolution; const double obj_vel_norm = std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y); @@ -1032,10 +929,10 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) lanes_polygon.target = utils::lane_change::create_polygon(lanes->target, 0.0, std::numeric_limits::max()); - const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto & params = common_data_ptr->lc_param_ptr->safety; const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - lanes->target, common_data_ptr->direction, lc_param_ptr->lane_expansion_left_offset, - lc_param_ptr->lane_expansion_right_offset); + lanes->target, common_data_ptr->direction, params.lane_expansion_left_offset, + params.lane_expansion_right_offset); lanes_polygon.expanded_target = utils::lane_change::create_polygon( expanded_target_lanes, 0.0, std::numeric_limits::max()); @@ -1184,7 +1081,7 @@ double get_min_dist_to_current_lanes_obj( for (const auto & object : filtered_objects.current_lane) { // check if stationary const auto obj_v = std::abs(object.initial_twist.linear.x); - if (obj_v > common_data_ptr->lc_param_ptr->stop_velocity_threshold) { + if (obj_v > common_data_ptr->lc_param_ptr->th_stop_velocity) { continue; } @@ -1297,7 +1194,7 @@ bool filter_target_lane_objects( const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto & vehicle_width = common_data_ptr->bpp_param_ptr->vehicle_info.vehicle_width_m; const auto & lanes_polygon = *common_data_ptr->lanes_polygon_ptr; - const auto stopped_obj_vel_th = common_data_ptr->lc_param_ptr->stopped_object_velocity_threshold; + const auto stopped_obj_vel_th = common_data_ptr->lc_param_ptr->safety.th_stopped_object_velocity; const auto is_lateral_far = std::invoke([&]() -> bool { const auto dist_object_to_current_lanes_center = diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 504de657eabda..555657fe34af9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -20,6 +20,8 @@ #include #include +#include +#include #include using autoware::behavior_path_planner::BehaviorPathPlannerNode; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp index 6f285190c4169..ea36e4dc6960a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -21,12 +21,15 @@ #include -#include "autoware_planning_msgs/msg/lanelet_route.hpp" +#include #include +#include #include +#include +using autoware::behavior_path_planner::FilteredLanesObjects; using autoware::behavior_path_planner::LaneChangeModuleManager; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::NormalLaneChange; @@ -40,6 +43,7 @@ using autoware::test_utils::get_absolute_path_to_config; using autoware::test_utils::get_absolute_path_to_lanelet_map; using autoware::test_utils::get_absolute_path_to_route; using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; @@ -64,8 +68,13 @@ class TestNormalLaneChange : public ::testing::Test ego_pose_ = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); planner_data_->self_odometry = set_odometry(ego_pose_); - planner_data_->dynamic_object = - std::make_shared(); + const auto objects_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_path_lane_change_module") + + "/test_data/test_object_filter.yaml"; + + YAML::Node yaml_node = YAML::LoadFile(objects_file); + const auto objects = autoware::test_utils::parse(yaml_node); + planner_data_->dynamic_object = std::make_shared(objects); } void init_module() @@ -119,7 +128,11 @@ class TestNormalLaneChange : public ::testing::Test auto route_handler_ptr = std::make_shared(map_bin_msg); const auto rh_test_route = get_absolute_path_to_route(autoware_route_handler_dir, lane_change_right_test_route_filename); - route_handler_ptr->setRoute(autoware::test_utils::parse(rh_test_route)); + if ( + const auto route_opt = + autoware::test_utils::parse>(rh_test_route)) { + route_handler_ptr->setRoute(*route_opt); + } return route_handler_ptr; } @@ -131,6 +144,11 @@ class TestNormalLaneChange : public ::testing::Test return std::make_shared(odom); } + [[nodiscard]] FilteredLanesObjects & get_filtered_objects() const + { + return normal_lane_change_->filtered_objects_; + } + void set_previous_approved_path() { normal_lane_change_->prev_module_output_.path = create_previous_approved_path(); @@ -205,13 +223,36 @@ TEST_F(TestNormalLaneChange, testGetPathWhenInvalid) constexpr auto is_approved = true; normal_lane_change_->update_lanes(!is_approved); normal_lane_change_->update_filtered_objects(); - normal_lane_change_->update_transient_data(); + normal_lane_change_->update_transient_data(!is_approved); normal_lane_change_->updateLaneChangeStatus(); const auto & lc_status = normal_lane_change_->getLaneChangeStatus(); ASSERT_FALSE(lc_status.is_valid_path); } +TEST_F(TestNormalLaneChange, testFilteredObjects) +{ + constexpr auto is_approved = true; + ego_pose_ = autoware::test_utils::createPose(1.0, 1.75, 0.0, 0.0, 0.0, 0.0); + planner_data_->self_odometry = set_odometry(ego_pose_); + set_previous_approved_path(); + + normal_lane_change_->update_lanes(!is_approved); + normal_lane_change_->update_filtered_objects(); + + const auto & filtered_objects = get_filtered_objects(); + + // Note: There's 1 stopping object in current lanes, however, it was filtered out. + const auto filtered_size = + filtered_objects.current_lane.size() + filtered_objects.target_lane_leading.size() + + filtered_objects.target_lane_trailing.size() + filtered_objects.others.size(); + EXPECT_EQ(filtered_size, planner_data_->dynamic_object->objects.size()); + EXPECT_EQ(filtered_objects.current_lane.size(), 0); + EXPECT_EQ(filtered_objects.target_lane_leading.size(), 2); + EXPECT_EQ(filtered_objects.target_lane_trailing.size(), 0); + EXPECT_EQ(filtered_objects.others.size(), 2); +} + TEST_F(TestNormalLaneChange, testGetPathWhenValid) { constexpr auto is_approved = true; @@ -220,7 +261,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenValid) set_previous_approved_path(); normal_lane_change_->update_lanes(!is_approved); normal_lane_change_->update_filtered_objects(); - normal_lane_change_->update_transient_data(); + normal_lane_change_->update_transient_data(!is_approved); const auto lane_change_required = normal_lane_change_->isLaneChangeRequired(); ASSERT_TRUE(lane_change_required); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test_data/test_object_filter.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test_data/test_object_filter.yaml new file mode 100644 index 0000000000000..3ef05a0cd1024 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test_data/test_object_filter.yaml @@ -0,0 +1,1838 @@ +header: + stamp: + sec: 1730959270 + nanosec: 700904396 + frame_id: map +objects: + - object_id: + uuid: + - 57 + - 234 + - 237 + - 159 + - 195 + - 229 + - 66 + - 71 + - 21 + - 44 + - 68 + - 122 + - 144 + - 106 + - 13 + - 190 + existence_probability: 0.0 + classification: + - label: 3 + probability: 1.0 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + covariance: + - 0.12314689855728007 + - -0.0061611584403754 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -0.0061611584403753975 + - 0.0028632078333398737 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0007922605681590912 + initial_twist_with_covariance: + twist: + linear: + x: 0.0022924032607949227 + y: 1.0376500541998356e-07 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 3.908788083083818e-08 + covariance: + - 0.7401171867683597 + - 3.350119623596881e-05 + - 0.0 + - 0.0 + - 0.0 + - 1.2619772541445709e-05 + - 3.350119623596881e-05 + - 4.254924999186505e-08 + - 0.0 + - 0.0 + - 0.0 + - 1.6028139799077814e-08 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 1.261977254144571e-05 + - 1.6028139799077817e-08 + - 0.0 + - 0.0 + - 0.0 + - 6.037738984069024e-09 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.0 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 0.0 + covariance: + - 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 + - 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 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + predicted_paths: + - path: + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + - position: + x: -26.277885137995664 + y: -1.894734886474639 + z: 0.8911920935795934 + orientation: + x: 0.0 + y: 0.0 + z: -0.025541935117105664 + w: 0.9996737515562133 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 1.0 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 10.61863710330581 + y: 2.6 + z: 1.8699817255447553 + - object_id: + uuid: + - 141 + - 149 + - 211 + - 87 + - 79 + - 101 + - 139 + - 123 + - 68 + - 8 + - 135 + - 54 + - 236 + - 22 + - 42 + - 245 + existence_probability: 0.0 + classification: + - label: 3 + probability: 1.0 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + covariance: + - 0.12346140759139482 + - 0.0001740656821558044 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0001740656821558044 + - 0.0025486884508984387 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0007923887288897377 + initial_twist_with_covariance: + twist: + linear: + x: 0.01149754330094817 + y: -3.1578956564191503e-07 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: -1.340071831009146e-07 + covariance: + - 0.7401171745434685 + - -2.0327885143452095e-05 + - 0.0 + - 0.0 + - 0.0 + - -8.6262591385362e-06 + - -2.0327885143452095e-05 + - 1.0315766321625746e-06 + - 0.0 + - 0.0 + - 0.0 + - 4.377556881837847e-07 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - -8.6262591385362e-06 + - 4.3775568818378476e-07 + - 0.0 + - 0.0 + - 0.0 + - 1.8576423366195293e-07 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.0 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 0.0 + covariance: + - 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 + - 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 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + predicted_paths: + - path: + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + - position: + x: -10.7607993411875 + y: -1.9485222301582967 + z: 1.8354702975479853 + orientation: + x: 0.0 + y: 0.0 + z: 0.0007335045608968457 + w: 0.9999997309854934 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 1.0 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 9.426048912739507 + y: 2.6538046848726022 + z: 3.370528331600673 + - object_id: + uuid: + - 71 + - 249 + - 231 + - 94 + - 71 + - 17 + - 61 + - 160 + - 244 + - 177 + - 239 + - 139 + - 84 + - 132 + - 224 + - 157 + existence_probability: 0.0 + classification: + - label: 3 + probability: 1.0 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + covariance: + - 0.12325225128472374 + - -0.004992467682066607 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -0.004992467682066607 + - 0.002755512431768385 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0007924339915606306 + initial_twist_with_covariance: + twist: + linear: + x: -0.008145866229493542 + y: 2.7177554489085472e-06 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 1.553003113662027e-06 + covariance: + - 0.7401113329295389 + - -0.00024692616733628655 + - 0.0 + - 0.0 + - 0.0 + - -0.0001411006670493066 + - -0.0002469261673362865 + - 6.212518114314515e-07 + - 0.0 + - 0.0 + - 0.0 + - 3.550010351036865e-07 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - -0.0001411006670493066 + - 3.5500103510368645e-07 + - 0.0 + - 0.0 + - 0.0 + - 2.028577343449637e-07 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.0 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 0.0 + covariance: + - 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 + - 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 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + predicted_paths: + - path: + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + - position: + x: 8.951931667607116 + y: 2.0177028806099195 + z: 1.9961166695683017 + orientation: + x: 0.0 + y: 0.0 + z: -0.02050547163810942 + w: 0.9997897407119652 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 1.0 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 7.0 + y: 2.5391492564805107 + z: 2.988733287063526 + - object_id: + uuid: + - 131 + - 104 + - 242 + - 151 + - 76 + - 97 + - 188 + - 81 + - 236 + - 166 + - 159 + - 74 + - 95 + - 82 + - 96 + - 160 + existence_probability: 0.0 + classification: + - label: 3 + probability: 1.0 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: -15.804813657329067 + y: 6.927734802768457 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598655 + w: 0.9996716560741284 + covariance: + - 0.12314192052841168 + - 0.006207305811351786 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.006207305811351787 + - 0.002868420675357726 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0007923396833076764 + initial_twist_with_covariance: + twist: + linear: + x: -0.008710890141645097 + y: -1.4990440656518e-06 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: -5.645388351563947e-07 + covariance: + - 0.7401166659649231 + - 0.00012736585105397946 + - 0.0 + - 0.0 + - 0.0 + - 4.7965880950572506e-05 + - 0.00012736585105397946 + - 6.178150020515808e-07 + - 0.0 + - 0.0 + - 0.0 + - 2.3266865170417227e-07 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.010000000000000002 + - 0.0 + - 4.796588095057249e-05 + - 2.326686517041723e-07 + - 0.0 + - 0.0 + - 0.0 + - 8.762283419158182e-08 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.0 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 0.0 + covariance: + - 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 + - 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 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + predicted_paths: + - path: + - position: + x: -15.804813657329067 + y: 6.927734802768457 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.809163344604654 + y: 6.92751092125665 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.813513031880241 + y: 6.927287039744844 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.81786271915583 + y: 6.927063158233037 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.822212406431417 + y: 6.92683927672123 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.826562093707004 + y: 6.926615395209423 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.830911780982591 + y: 6.926391513697617 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.835261468258178 + y: 6.926167632185811 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.839611155533767 + y: 6.925943750674004 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.843960842809354 + y: 6.925719869162197 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.848310530084941 + y: 6.9254959876503905 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.852660217360528 + y: 6.925272106138584 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.857009904636115 + y: 6.925048224626777 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.861359591911704 + y: 6.92482434311497 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.865709279187291 + y: 6.924600461603164 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.870058966462878 + y: 6.924376580091357 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.874408653738465 + y: 6.92415269857955 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.878758341014052 + y: 6.923928817067743 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.88310802828964 + y: 6.923704935555937 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.887457715565228 + y: 6.92348105404413 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.891807402840815 + y: 6.923257172532324 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.896157090116402 + y: 6.923033291020517 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.900506777391989 + y: 6.9228094095087105 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.904856464667578 + y: 6.922585527996904 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.909206151943165 + y: 6.922361646485097 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.913555839218752 + y: 6.92213776497329 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.917905526494339 + y: 6.9219138834614835 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.922255213769926 + y: 6.921690001949677 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.926604901045513 + y: 6.92146612043787 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.930954588321102 + y: 6.921242238926063 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + - position: + x: -15.935304275596689 + y: 6.9210183574142565 + z: 1.7960623786923189 + orientation: + x: 0.0 + y: 0.0 + z: 0.025623817865598652 + w: 0.9996716560741286 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 1.0 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 10.621370735187906 + y: 2.6779142303492427 + z: 3.4752818839000423 diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index bd5b391127467..041b7ad20a107 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -25,8 +25,13 @@ #include +#include +#include #include #include +#include +#include +#include namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.cpp index f4f368008c472..d1602432b19c2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "input.hpp" +#include + namespace autoware::behavior_path_planner { using autoware_planning_msgs::msg::PathPoint; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp index b7d992d9254dc..63b7ccf3fff12 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp @@ -21,6 +21,8 @@ #include #include +#include +#include #include using autoware::behavior_path_planner::BehaviorPathPlannerNode; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp index 36e2914571c1f..3d6a76f6807a9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp @@ -23,6 +23,8 @@ #include #include +#include + using autoware::behavior_path_planner::PathWithLaneId; using autoware::behavior_path_planner::Pose; using autoware::behavior_path_planner::utils::FrenetPoint; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index d6d37c3f581e4..7162f49b450e8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -137,9 +137,21 @@ double l2Norm(const Vector3 vector); double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets); +/** + * @brief Calculates the distance to the next intersection. + * @param current_pose Ego pose. + * @param lanelets Lanelets to check. + * @return Distance. + */ double getDistanceToNextIntersection( const Pose & current_pose, const lanelet::ConstLanelets & lanelets); +/** + * @brief Calculates the distance to the next crosswalk. + * @param current_pose Ego pose. + * @param lanelets Lanelets to check. + * @return Distance. + */ double getDistanceToCrosswalk( const Pose & current_pose, const lanelet::ConstLanelets & lanelets, const lanelet::routing::RoutingGraphContainer & overall_graphs); @@ -236,6 +248,15 @@ bool set_goal( */ const Pose refineGoal(const Pose & goal, const lanelet::ConstLanelet & goal_lanelet); +/** + * @brief Recreate the path with a given goal pose. + * @param search_radius_range Searching radius. + * @param search_rad_range Searching angle. + * @param input Input path. + * @param goal Goal pose. + * @param goal_lane_id Lane ID of goal lanelet. + * @return Recreated path + */ PathWithLaneId refinePathForGoal( const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, const Pose & goal, const int64_t goal_lane_id); @@ -243,8 +264,22 @@ PathWithLaneId refinePathForGoal( bool isAllowedGoalModification(const std::shared_ptr & route_handler); bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); +/** + * @brief Checks if the given pose is inside the given lanes. + * @param pose Ego pose. + * @param lanes Lanelets to check. + * @return True if the ego pose is inside the lanes. + */ bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes); +/** + * @brief Checks if the given pose is inside the given lane within certain yaw difference. + * @param current_pose Ego pose. + * @param lanelet Lanelet to check. + * @param yaw_threshold Yaw angle difference threshold. + * @param radius Search radius + * @return True if the ego pose is inside the lane. + */ bool isInLaneletWithYawThreshold( const Pose & current_pose, const lanelet::ConstLanelet & lanelet, const double yaw_threshold, const double radius = 0.0); @@ -254,6 +289,14 @@ bool isEgoOutOfRoute( const std::optional & modified_goal, const std::shared_ptr & route_handler); +/** + * @brief Checks if the given pose is inside the original lane. + * @param current_lanes Original lanes. + * @param current_pose Ego pose. + * @param common_param Parameters used for behavior path planner. + * @param outer_margin Allowed margin. + * @return True if the ego pose is inside the original lane. + */ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); @@ -268,18 +311,48 @@ bool isEgoWithinOriginalLane( std::shared_ptr generateCenterLinePath( const std::shared_ptr & planner_data); +/** + * @brief Inserts a stop point with given length + * @param length Distance to insert stop point. + * @param path Original path. + * @return Inserted stop point. + */ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); +/** + * @brief Calculates distance to lane boundary. + * @param lanelet Target lanelet. + * @param position Ego position. + * @param left_side Whether to check left side boundary. + * @return Distance to boundary. + */ double getSignedDistanceFromLaneBoundary( const lanelet::ConstLanelet & lanelet, const Point & position, const bool left_side); + double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose, const bool left_side); + +/** + * @brief Calculates distance to lane boundary. + * @param lanelet Target lanelet. + * @param vehicle_width Ego vehicle width. + * @param base_link2front Ego vehicle distance from base link to front. + * @param base_link2rear Ego vehicle distance from base link to rear. + * @param vehicle_pose Ego vehicle pose. + * @param left_side Whether to check left side boundary. + * @return Distance to boundary. + */ std::optional getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, const double base_link2rear, const Pose & vehicle_pose, const bool left_side); // misc +/** + * @brief Convert lanelet to 2D polygon. + * @param lanelet Target lanelet. + * @return Polygon + */ Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet); Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon); @@ -316,10 +389,27 @@ lanelet::ConstLanelets extendPrevLane( lanelet::ConstLanelets extendLanes( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); +/** + * @brief Retrieves sequences of preceding lanelets from the target lanes. + * @param route_handler Reference to the route handler. + * @param target_lanes The set of target lanelets. + * @param current_pose The current pose of ego vehicle. + * @param backward_length The backward search length [m]. + * @return A vector of lanelet sequences that precede the target lanes + */ std::vector getPrecedingLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length); +/** + * @brief Retrieves all preceding lanelets as a flat list. + * @param route_handler Reference to the route handler. + * @param target_lanes The set of target lanelets. + * @param current_pose The current pose of ego vehicle. + * @param backward_length The backward search length [m]. + * @return Preceding lanelets within the specified backward length. + */ + lanelet::ConstLanelets getBackwardLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length); @@ -335,17 +425,41 @@ lanelet::ConstLanelets getExtendedCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const double backward_length, const double forward_length, const bool forward_only_in_route); +/** + * @brief Calculates the sequence of lanelets around a given pose within a specified range. + * @param route_handler A shared pointer to the RouteHandler for accessing route and lanelet + * information. + * @param pose The reference pose to locate the lanelets around. + * @param forward_length The length of the route to extend forward from the reference pose. + * @param backward_length The length of the route to extend backward from the reference pose. + * @param dist_threshold The maximum allowable distance to consider a lanelet as closest. + * @param yaw_threshold The maximum allowable yaw difference (in radians) for lanelet matching. + * @return A sequence of lanelets around the given pose. + */ lanelet::ConstLanelets calcLaneAroundPose( - const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, + const std::shared_ptr & route_handler, const geometry_msgs::msg::Pose & pose, const double forward_length, const double backward_length, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); +/** + * @brief Checks whether the relative angles between consecutive triplets of points in a path remain + * within a specified threshold. + * @param path Input path. + * @param angle_threshold The maximum allowable angle in radians. + * @return True if all relative angles are within the threshold or the path has fewer than three + * points. + */ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); +/** + * @brief Converts camel case string to snake case string. + * @param input_str Input string. + * @return String + */ std::string convertToSnakeCase(const std::string & input_str); std::optional getPolygonByPoint( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_interface.cpp index a6c480e130d7a..d7ecda3b92680 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_interface.cpp @@ -14,6 +14,8 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include + namespace autoware::behavior_path_planner { void SceneModuleInterface::setDrivableLanes(const std::vector & drivable_lanes) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp index 7b362d9a8bafa..b91db3a740cfe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp @@ -14,6 +14,10 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include +#include +#include + namespace autoware::behavior_path_planner { void SceneModuleManagerInterface::initInterface( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp index 454d8753b0e62..e6ccfc74866c4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp @@ -16,6 +16,8 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include + namespace autoware::behavior_path_planner { std::shared_ptr SceneModuleVisitor::getAvoidanceModuleDebugMsg() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index 90693aa65e661..7029c15e45b6e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -27,6 +27,8 @@ #include #include +#include +#include namespace marker_utils { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index 22d80da2cfa67..d513017c221cf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -26,10 +26,14 @@ #include #include +#include #include +#include #include +#include #include #include +#include namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 648bb0a17622c..1076af002fb28 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -30,7 +30,11 @@ #include +#include +#include #include +#include +#include namespace autoware::behavior_path_planner::drivable_area_expansion { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp index 280060334cfd4..4cd7467997669 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp @@ -20,6 +20,8 @@ #include #include +#include +#include namespace autoware::behavior_path_planner::drivable_area_expansion { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 1c6295f3e70ef..0768c620dd054 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -29,6 +29,14 @@ #include #include +#include +#include +#include +#include +#include +#include +#include + namespace { template diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index b18858e9dad04..789741153bf4a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -27,6 +27,9 @@ #include +#include +#include +#include #include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp index 5fea6b501ba24..18dee25919f98 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -19,6 +19,11 @@ #include #include +#include +#include +#include +#include + namespace autoware::behavior_path_planner::utils::parking_departure { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 3c1d9f194aeec..1caf287ee5386 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -23,6 +23,10 @@ #include #include +#include +#include +#include +#include namespace autoware::behavior_path_planner::utils::path_safety_checker::filter { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 04b61b86dfa02..5ae62f51bdb52 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -28,8 +28,12 @@ #include +#include #include #include +#include +#include +#include namespace autoware::behavior_path_planner::utils::path_safety_checker { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 4fe7765366c08..6d15f387fba72 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 1e2ad668bf28f..8195b49ae662c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -27,7 +27,9 @@ #include #include +#include #include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp index c31bddd921f99..305519279a9ea 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -16,6 +16,9 @@ #include #include +#include +#include + namespace autoware::behavior_path_planner::utils::traffic_light { using autoware::motion_utils::calcSignedArcLength; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index a546c0026d4d6..9b99a5ef7b31d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -33,7 +33,9 @@ #include #include #include +#include #include +#include #include namespace @@ -380,9 +382,8 @@ PathWithLaneId refinePathForGoal( search_radius_range, search_rad_range, filtered_path, goal, goal_lane_id, &path_with_goal)) { return path_with_goal; - } else { - return filtered_path; } + return filtered_path; } bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes) @@ -441,9 +442,8 @@ bool isEgoOutOfRoute( RCLCPP_WARN_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("util"), "ego pose is beyond goal"); return true; - } else { - return false; } + return false; } // If ego vehicle is out of the closest lanelet, return true @@ -466,11 +466,7 @@ bool isEgoOutOfRoute( return false; }); - if (!is_in_shoulder_lane && !is_in_road_lane) { - return true; - } - - return false; + return !is_in_shoulder_lane && !is_in_road_lane; } bool isEgoWithinOriginalLane( @@ -1359,8 +1355,9 @@ lanelet::ConstLanelets getBackwardLanelets( } lanelet::ConstLanelets calcLaneAroundPose( - const std::shared_ptr route_handler, const Pose & pose, const double forward_length, - const double backward_length, const double dist_threshold, const double yaw_threshold) + const std::shared_ptr & route_handler, const Pose & pose, + const double forward_length, const double backward_length, const double dist_threshold, + const double yaw_threshold) { lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithConstrainsWithinRoute( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp index 21784757e469e..b8e3cca852a0a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp @@ -21,6 +21,8 @@ #include #include +#include + using autoware::behavior_path_planner::drivable_area_expansion::LineString2d; using autoware::behavior_path_planner::drivable_area_expansion::Point2d; using autoware::behavior_path_planner::drivable_area_expansion::Segment2d; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp index 88ed968d27b64..8c732a972a212 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp @@ -14,12 +14,22 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include +#include + #include #include +#include + +#include +#include +#include +#include using PredictedObject = autoware_perception_msgs::msg::PredictedObject; using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; @@ -32,10 +42,34 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; +using autoware::test_utils::make_lanelet; using autoware::universe_utils::createPoint; +using autoware::universe_utils::createVector3; constexpr double epsilon = 1e-6; +const auto intersection_map = + autoware::test_utils::make_map_bin_msg(autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "intersection/lanelet2_map.osm")); + +PredictedObject create_bounding_box_object( + const geometry_msgs::msg::Pose pose, + const geometry_msgs::msg::Vector3 velocity = geometry_msgs::msg::Vector3(), + const double x_dimension = 1.0, const double y_dimension = 1.0, + const std::vector & classification = std::vector()) +{ + PredictedObject object; + object.object_id = autoware::universe_utils::generateUUID(); + object.kinematics.initial_pose_with_covariance.pose = pose; + object.kinematics.initial_twist_with_covariance.twist.linear = velocity; + object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + object.shape.dimensions.x = x_dimension; + object.shape.dimensions.y = y_dimension; + object.classification = classification; + + return object; +} + std::vector trajectory_to_path_with_lane_id(const Trajectory & trajectory) { std::vector path_with_lane_id; @@ -81,8 +115,7 @@ TEST(BehaviorPathPlanningObjectsFiltering, position_filter) using autoware::behavior_path_planner::utils::path_safety_checker::filter::position_filter; auto current_pos = createPoint(0.0, 0.0, 0.0); - PredictedObject object; - object.kinematics.initial_pose_with_covariance.pose = createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0); + PredictedObject object = create_bounding_box_object(createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)); auto straight_trajectory = generateTrajectory(20, 1.0); double forward_distance = 20.0; double backward_distance = 1.0; @@ -129,23 +162,101 @@ TEST(BehaviorPathPlanningObjectsFiltering, is_within_circle) EXPECT_TRUE(is_within_circle(object_pos, ref_point, search_radius)); } +TEST(BehaviorPathPlanningObjectsFiltering, isCentroidWithinLanelet) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::isCentroidWithinLanelet; + using autoware::behavior_path_planner::utils::path_safety_checker::isCentroidWithinLanelets; + + auto object = create_bounding_box_object(createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0)); + auto lanelet = make_lanelet({0.0, 1.0}, {5.0, 1.0}, {0.0, -1.0}, {5.0, -1.0}); + double yaw_threshold = M_PI_2; + + EXPECT_TRUE(isCentroidWithinLanelet(object, lanelet, yaw_threshold)); + + object.kinematics.initial_pose_with_covariance.pose.position.x = 8.0; + EXPECT_FALSE(isCentroidWithinLanelet(object, lanelet, yaw_threshold)); + + lanelet::ConstLanelets target_lanelets; + target_lanelets.push_back(lanelet); + target_lanelets.push_back(make_lanelet({5.0, 1.0}, {10.0, 1.0}, {5.0, -1.0}, {10.0, -1.0})); + EXPECT_TRUE(isCentroidWithinLanelets(object, target_lanelets)); +} + +TEST(BehaviorPathPlanningObjectsFiltering, isPolygonOverlapLanelet) +{ + using autoware::behavior_path_planner::utils::toPolygon2d; + using autoware::behavior_path_planner::utils::path_safety_checker::isPolygonOverlapLanelet; + + PredictedObject object = create_bounding_box_object(createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0)); + + auto lanelet = make_lanelet({0.0, 1.0}, {5.0, 1.0}, {0.0, -1.0}, {5.0, -1.0}); + double yaw_threshold = M_PI_2; + + EXPECT_TRUE(isPolygonOverlapLanelet(object, lanelet.polygon2d().basicPolygon())); + EXPECT_TRUE(isPolygonOverlapLanelet(object, toPolygon2d(lanelet))); + EXPECT_TRUE(isPolygonOverlapLanelet(object, lanelet, yaw_threshold)); + + object.kinematics.initial_pose_with_covariance.pose = createPose(10.0, 10.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_FALSE(isPolygonOverlapLanelet(object, lanelet.polygon2d().basicPolygon())); + EXPECT_FALSE(isPolygonOverlapLanelet(object, toPolygon2d(lanelet))); + EXPECT_FALSE(isPolygonOverlapLanelet(object, lanelet, yaw_threshold)); +} + +TEST(BehaviorPathPlanningObjectsFiltering, filterObjects) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filterObjects; + using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; + using autoware::universe_utils::createVector3; + + std::shared_ptr objects = std::make_shared(); + std::shared_ptr route_handler = + std::make_shared(); + std::shared_ptr params = std::make_shared(); + params->ignore_object_velocity_threshold = false; + params->object_check_forward_distance = 20.0; + params->object_check_backward_distance = 10.0; + params->object_types_to_check.check_car = true; + route_handler->setMap(intersection_map); + lanelet::ConstLanelets current_lanes; + + current_lanes.push_back(route_handler->getLaneletsFromId(1000)); + current_lanes.push_back(route_handler->getLaneletsFromId(1010)); + auto current_pose = createPoint(360.22, 600.51, 0.0); + + EXPECT_TRUE( + filterObjects(objects, route_handler, current_lanes, current_pose, params).objects.empty()); + + ObjectClassification classification; + classification.label = ObjectClassification::Type::CAR; + classification.probability = 1.0; + + auto target_object = create_bounding_box_object( + createPose(360.22, 605.51, 0.0, 0.0, 0.0, 0.0), createVector3(1.0, 1.0, 0.0)); + target_object.classification.push_back(classification); + auto other_object = create_bounding_box_object( + createPose(370.22, 600.51, 0.0, 0.0, 0.0, 0.0), createVector3(1.0, 1.0, 0.0)); + other_object.classification.push_back(classification); + + objects->objects.push_back(target_object); + objects->objects.push_back(other_object); + + auto filtered_object = filterObjects(objects, route_handler, current_lanes, current_pose, params); + EXPECT_FALSE(filtered_object.objects.empty()); + EXPECT_EQ(filtered_object.objects.front().object_id, target_object.object_id); +} + TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByVelocity) { using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsByVelocity; PredictedObjects objects; - PredictedObject slow_obj; - PredictedObject fast_obj; + auto slow_obj = create_bounding_box_object( + createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(2.0, 0.0, 0.0)); + auto fast_obj = create_bounding_box_object( + createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(10.0, 0.0, 0.0)); double vel_thr = 5.0; - slow_obj.object_id = autoware::universe_utils::generateUUID(); - slow_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; - slow_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; objects.objects.push_back(slow_obj); - - fast_obj.object_id = autoware::universe_utils::generateUUID(); - fast_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 10.0; - fast_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; objects.objects.push_back(fast_obj); auto filtered_obj = filterObjectsByVelocity(objects, vel_thr, false); @@ -176,17 +287,12 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByPosition) double search_radius = 10.0; PredictedObjects objects; - PredictedObject far_obj; - PredictedObject near_obj; + auto far_obj = create_bounding_box_object(createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + auto near_obj = create_bounding_box_object(createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - near_obj.object_id = autoware::universe_utils::generateUUID(); - near_obj.kinematics.initial_pose_with_covariance.pose = createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0); objects.objects.push_back(near_obj); - auto target_uuid = near_obj.object_id; - - far_obj.object_id = autoware::universe_utils::generateUUID(); - far_obj.kinematics.initial_pose_with_covariance.pose = createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0); objects.objects.push_back(far_obj); + auto target_uuid = near_obj.object_id; filterObjectsByPosition(objects, straight_path, current_pos, forward_distance, backward_distance); @@ -202,6 +308,108 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByPosition) EXPECT_EQ(objects.objects.front().object_id, target_uuid); } +TEST(BehaviorPathPlanningObjectsFiltering, separateObjectsByLanelets) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::isPolygonOverlapLanelet; + using autoware::behavior_path_planner::utils::path_safety_checker:: + separateObjectIndicesByLanelets; + using autoware::behavior_path_planner::utils::path_safety_checker::separateObjectsByLanelets; + + double yaw_threshold = M_PI_2; + + auto target_object = create_bounding_box_object(createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0)); + auto other_object = create_bounding_box_object(createPose(-1.5, 0.0, 0.0, 0.0, 0.0, 0.0)); + + PredictedObjects objects; + objects.objects.push_back(target_object); + objects.objects.push_back(other_object); + + lanelet::ConstLanelets target_lanelets; + { + auto object_indices = separateObjectIndicesByLanelets( + objects, target_lanelets, + [](const auto & obj, const auto & lane, const auto & yaw_threshold) { + return isPolygonOverlapLanelet(obj, lane, yaw_threshold); + }, + yaw_threshold); + EXPECT_TRUE(object_indices.first.empty()); + EXPECT_TRUE(object_indices.second.empty()); + } + { + target_lanelets.push_back(make_lanelet({0.0, 1.0}, {5.0, 1.0}, {0.0, -1.0}, {5.0, -1.0})); + target_lanelets.push_back(make_lanelet({5.0, 1.0}, {10.0, 1.0}, {5.0, -1.0}, {10.0, -1.0})); + auto object_indices = separateObjectIndicesByLanelets( + objects, target_lanelets, + [](const auto & obj, const auto & lane, const auto & yaw_threshold) { + return isPolygonOverlapLanelet(obj, lane, yaw_threshold); + }, + yaw_threshold); + EXPECT_FALSE(object_indices.first.empty()); + EXPECT_FALSE(object_indices.second.empty()); + EXPECT_EQ(object_indices.first.front(), 0); + EXPECT_EQ(object_indices.second.front(), 1); + + auto filtered_object = separateObjectsByLanelets( + objects, target_lanelets, + [](const auto & obj, const auto & lane, const auto & yaw_threshold) { + return isPolygonOverlapLanelet(obj, lane, yaw_threshold); + }, + yaw_threshold); + EXPECT_FALSE(filtered_object.first.objects.empty()); + EXPECT_FALSE(filtered_object.second.objects.empty()); + EXPECT_EQ(filtered_object.first.objects.front().object_id, target_object.object_id); + EXPECT_EQ(filtered_object.second.objects.front().object_id, other_object.object_id); + } +} + +TEST(BehaviorPathPlanningObjectsFiltering, getPredictedPathFromObj) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::getPredictedPathFromObj; + using autoware::behavior_path_planner::utils::path_safety_checker:: + PoseWithVelocityAndPolygonStamped; + using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; + + autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject object; + std::vector predicted_paths; + PredictedPathWithPolygon predicted_path; + + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + shape.dimensions.x = 1.0; + shape.dimensions.y = 1.0; + + double velocity = 1.0; + + const auto path = [&](geometry_msgs::msg::Pose initial_pose) { + std::vector path; + geometry_msgs::msg::Pose pose; + for (size_t i = 0; i < 10; i++) { + auto time = static_cast(i); + pose.position.x = initial_pose.position.x + time * velocity; + pose.position.y = initial_pose.position.y; + PoseWithVelocityAndPolygonStamped obj_pose_with_poly( + time, pose, velocity, autoware::universe_utils::toPolygon2d(pose, shape)); + path.push_back(obj_pose_with_poly); + } + return path; + }; + + for (size_t i = 0; i < 2; i++) { + predicted_path.path = path(createPose(0.0, static_cast(i), 0.0, 0.0, 0.0, 0.0)); + predicted_path.confidence = 0.1f * (static_cast(i) + 1.0f); + predicted_paths.push_back(predicted_path); + } + object.predicted_paths = predicted_paths; + + bool use_all_predicted_path = true; + EXPECT_EQ(getPredictedPathFromObj(object, use_all_predicted_path).size(), 2); + + use_all_predicted_path = false; + auto extracted_path = getPredictedPathFromObj(object, use_all_predicted_path); + EXPECT_EQ(extracted_path.size(), 1); + EXPECT_DOUBLE_EQ(extracted_path.front().path.front().pose.position.y, 1.0); +} + TEST(BehaviorPathPlanningObjectsFiltering, createPredictedPath) { using autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath; @@ -257,22 +465,14 @@ TEST(BehaviorPathPlanningObjectsFiltering, transform) { using autoware::behavior_path_planner::utils::path_safety_checker::transform; auto velocity = autoware::universe_utils::createVector3(2.0, 0.0, 0.0); - auto angular = autoware::universe_utils::createVector3(0.0, 0.0, 0.0); - PredictedObject obj; - obj.object_id = autoware::universe_utils::generateUUID(); - obj.kinematics.initial_pose_with_covariance.pose = createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0); - obj.kinematics.initial_twist_with_covariance.twist = - autoware::universe_utils::createTwist(velocity, angular); + auto obj = create_bounding_box_object( + createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(2.0, 0.0, 0.0), 2.0, 1.0); autoware_perception_msgs::msg::PredictedPath predicted_path; auto straight_path = trajectory_to_predicted_path(generateTrajectory(5, 1.0)); straight_path.confidence = 0.6; straight_path.time_step.sec = 1.0; obj.kinematics.predicted_paths.push_back(straight_path); - obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; - obj.shape.dimensions.x = 2.0; - obj.shape.dimensions.y = 1.0; - obj.shape.dimensions.z = 1.0; double safety_check_time_horizon = 2.0; double safety_check_time_resolution = 0.5; @@ -356,6 +556,50 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByClass) EXPECT_TRUE(objects.objects.empty()); } +TEST(BehaviorPathPlanningObjectsFiltering, createTargetObjectsOnLane) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::createTargetObjectsOnLane; + using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; + using autoware::universe_utils::createVector3; + + PredictedObjects objects; + std::shared_ptr route_handler = + std::make_shared(); + std::shared_ptr params = std::make_shared(); + params->object_lane_configuration = {true, true, true, true, true}; + params->include_opposite_lane = true; + params->invert_opposite_lane = false; + params->safety_check_time_horizon = 10.0; + params->safety_check_time_resolution = 1.0; + route_handler->setMap(intersection_map); + lanelet::ConstLanelets current_lanes; + + current_lanes.push_back(route_handler->getLaneletsFromId(1001)); + current_lanes.push_back(route_handler->getLaneletsFromId(1011)); + + ObjectClassification classification; + classification.label = ObjectClassification::Type::CAR; + classification.probability = 1.0; + + PredictedObject current_lane_object = + create_bounding_box_object(createPose(363.64, 565.03, 0.0, 0.0, 0.0, 0.0)); + PredictedObject right_lane_object = + create_bounding_box_object(createPose(366.91, 523.47, 0.0, 0.0, 0.0, 0.0)); + + objects.objects.push_back(current_lane_object); + objects.objects.push_back(right_lane_object); + + auto target_objects_on_lane = + createTargetObjectsOnLane(current_lanes, route_handler, objects, params); + EXPECT_FALSE(target_objects_on_lane.on_current_lane.empty()); + EXPECT_FALSE(target_objects_on_lane.on_right_lane.empty()); + EXPECT_TRUE(target_objects_on_lane.on_left_lane.empty()); + EXPECT_TRUE(target_objects_on_lane.on_other_lane.empty()); + + EXPECT_EQ(target_objects_on_lane.on_current_lane.front().uuid, current_lane_object.object_id); + EXPECT_EQ(target_objects_on_lane.on_right_lane.front().uuid, right_lane_object.object_id); +} + TEST(BehaviorPathPlanningObjectsFiltering, isTargetObjectType) { using autoware::behavior_path_planner::utils::path_safety_checker::isTargetObjectType; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp index 4cb1b8ef14d5a..2b43cdaebd14d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp @@ -23,6 +23,9 @@ #include #include +#include +#include +#include constexpr double epsilon = 1e-6; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_shifter.cpp index 25fc533fc53a4..eacd1b086cd3b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_shifter.cpp @@ -21,6 +21,9 @@ #include +#include +#include + namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp index d96b30cccdfaa..562e8abfd432a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp @@ -22,6 +22,7 @@ #include #include +#include using tier4_planning_msgs::msg::PathWithLaneId; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp index ba3141a89b26a..ab6076b328d14 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp @@ -32,6 +32,7 @@ #include #include +#include const auto intersection_map = autoware::test_utils::make_map_bin_msg(autoware::test_utils::get_absolute_path_to_lanelet_map( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp index 4326cfdb8e6be..13fec41092cc8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp @@ -15,8 +15,16 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include +#include + +#include +#include +#include +#include +#include using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::PredictedObject; @@ -25,11 +33,55 @@ using autoware_planning_msgs::msg::Trajectory; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; +using autoware::behavior_path_planner::PlannerData; +using autoware_planning_msgs::msg::LaneletRoute; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; -TEST(BehaviorPathPlanningUtilTest, l2Norm) +class BehaviorPathPlanningUtilTest : public ::testing::Test +{ +protected: + void SetUp() override + { + planner_data_ = std::make_shared(); + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner_common") + + "/test_data/test_traffic_light.yaml"; + YAML::Node config = YAML::LoadFile(test_data_file); + + set_current_pose(config); + set_route_handler(config); + } + + void set_current_pose(YAML::Node config) + { + const auto self_odometry = + autoware::test_utils::parse(config["self_odometry"]); + planner_data_->self_odometry = std::make_shared(self_odometry); + } + + void set_route_handler(YAML::Node config) + { + const auto route = autoware::test_utils::parse(config["route"]); + const auto intersection_map = + autoware::test_utils::make_map_bin_msg(autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "intersection/lanelet2_map.osm")); + planner_data_->route_handler->setMap(intersection_map); + planner_data_->route_handler->setRoute(route); + + for (const auto & segment : route.segments) { + current_lanelets.push_back( + planner_data_->route_handler->getLaneletsFromId(segment.preferred_primitive.id)); + } + } + + std::shared_ptr planner_data_; + lanelet::ConstLanelets current_lanelets; + const double epsilon = 1e-06; +}; + +TEST_F(BehaviorPathPlanningUtilTest, l2Norm) { using autoware::behavior_path_planner::utils::l2Norm; @@ -42,7 +94,7 @@ TEST(BehaviorPathPlanningUtilTest, l2Norm) EXPECT_DOUBLE_EQ(norm, 3.0); } -TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenPathFootprintsAndObjects) +TEST_F(BehaviorPathPlanningUtilTest, checkCollisionBetweenPathFootprintsAndObjects) { using autoware::behavior_path_planner::utils::checkCollisionBetweenPathFootprintsAndObjects; @@ -75,7 +127,7 @@ TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenPathFootprintsAndObjects checkCollisionBetweenPathFootprintsAndObjects(base_footprint, ego_path, objs, margin)); } -TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenFootprintAndObjects) +TEST_F(BehaviorPathPlanningUtilTest, checkCollisionBetweenFootprintAndObjects) { using autoware::behavior_path_planner::utils::checkCollisionBetweenFootprintAndObjects; @@ -105,7 +157,7 @@ TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenFootprintAndObjects) EXPECT_TRUE(checkCollisionBetweenFootprintAndObjects(base_footprint, ego_pose, objs, margin)); } -TEST(BehaviorPathPlanningUtilTest, calcLateralDistanceFromEgoToObject) +TEST_F(BehaviorPathPlanningUtilTest, calcLateralDistanceFromEgoToObject) { using autoware::behavior_path_planner::utils::calcLateralDistanceFromEgoToObject; @@ -130,7 +182,7 @@ TEST(BehaviorPathPlanningUtilTest, calcLateralDistanceFromEgoToObject) EXPECT_DOUBLE_EQ(calcLateralDistanceFromEgoToObject(ego_pose, vehicle_width, obj), 3.0); } -TEST(BehaviorPathPlanningUtilTest, calc_longitudinal_distance_from_ego_to_object) +TEST_F(BehaviorPathPlanningUtilTest, calc_longitudinal_distance_from_ego_to_object) { using autoware::behavior_path_planner::utils::calc_longitudinal_distance_from_ego_to_object; @@ -164,7 +216,7 @@ TEST(BehaviorPathPlanningUtilTest, calc_longitudinal_distance_from_ego_to_object 2.0); } -TEST(BehaviorPathPlanningUtilTest, calcLongitudinalDistanceFromEgoToObjects) +TEST_F(BehaviorPathPlanningUtilTest, calcLongitudinalDistanceFromEgoToObjects) { using autoware::behavior_path_planner::utils::calcLongitudinalDistanceFromEgoToObjects; @@ -198,7 +250,206 @@ TEST(BehaviorPathPlanningUtilTest, calcLongitudinalDistanceFromEgoToObjects) calcLongitudinalDistanceFromEgoToObjects(ego_pose, base_link2front, base_link2rear, objs), 3.0); } -TEST(BehaviorPathPlanningUtilTest, getHighestProbLabel) +TEST_F(BehaviorPathPlanningUtilTest, refineGoal) +{ + using autoware::behavior_path_planner::utils::refineGoal; + + { + const auto goal_lanelet = current_lanelets.front(); + const auto goal_pose = planner_data_->self_odometry->pose.pose; + const auto refined_pose = refineGoal(goal_pose, goal_lanelet); + EXPECT_DOUBLE_EQ(refined_pose.position.x, goal_pose.position.x); + EXPECT_DOUBLE_EQ(refined_pose.position.y, goal_pose.position.y); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, refinePathForGoal) +{ + using autoware::behavior_path_planner::utils::refinePathForGoal; + + auto path = generateTrajectory(10, 1.0, 3.0); + const double search_rad_range = M_PI; + const auto goal_pose = createPose(5.2, 0.0, 0.0, 0.0, 0.0, 0.0); + const int64_t goal_lane_id = 5; + { + const double search_radius_range = 1.0; + const auto refined_path = + refinePathForGoal(search_radius_range, search_rad_range, path, goal_pose, goal_lane_id); + EXPECT_EQ(refined_path.points.size(), 7); + EXPECT_DOUBLE_EQ(refined_path.points.back().point.longitudinal_velocity_mps, 0.0); + EXPECT_DOUBLE_EQ(refined_path.points.back().point.pose.position.x, 5.2); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, isInLanelets) +{ + using autoware::behavior_path_planner::utils::isInLanelets; + using autoware::behavior_path_planner::utils::isInLaneletWithYawThreshold; + + { + const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_FALSE(isInLanelets(pose, current_lanelets)); + EXPECT_FALSE(isInLaneletWithYawThreshold(pose, current_lanelets.front(), M_PI_2)); + } + { + EXPECT_TRUE(isInLanelets(planner_data_->self_odometry->pose.pose, current_lanelets)); + EXPECT_TRUE(isInLaneletWithYawThreshold( + planner_data_->self_odometry->pose.pose, current_lanelets.front(), M_PI_2)); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, isEgoWithinOriginalLane) +{ + using autoware::behavior_path_planner::utils::isEgoWithinOriginalLane; + BehaviorPathPlannerParameters common_param; + common_param.vehicle_width = 1.0; + common_param.base_link2front = 1.0; + common_param.base_link2rear = 1.0; + + { + const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_FALSE(isEgoWithinOriginalLane(current_lanelets, pose, common_param)); + } + { + EXPECT_TRUE(isEgoWithinOriginalLane( + current_lanelets, planner_data_->self_odometry->pose.pose, common_param)); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getDistanceToNextIntersection) +{ + using autoware::behavior_path_planner::utils::getDistanceToNextIntersection; + + const auto current_pose = planner_data_->self_odometry->pose.pose; + { + const lanelet::ConstLanelets empty_lanelets; + EXPECT_DOUBLE_EQ( + getDistanceToNextIntersection(current_pose, empty_lanelets), + std::numeric_limits::infinity()); + } + { + EXPECT_NEAR( + getDistanceToNextIntersection(current_pose, current_lanelets), 117.1599371, epsilon); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getDistanceToCrosswalk) +{ + using autoware::behavior_path_planner::utils::getDistanceToCrosswalk; + + const auto current_pose = planner_data_->self_odometry->pose.pose; + const auto routing_graph = planner_data_->route_handler->getOverallGraphPtr(); + { + const lanelet::ConstLanelets empty_lanelets; + EXPECT_DOUBLE_EQ( + getDistanceToCrosswalk(current_pose, empty_lanelets, *routing_graph), + std::numeric_limits::infinity()); + } + { + EXPECT_NEAR( + getDistanceToCrosswalk(current_pose, current_lanelets, *routing_graph), 120.4423193, epsilon); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, insertStopPoint) +{ + using autoware::behavior_path_planner::utils::insertStopPoint; + + { + const double length = 100.0; + auto path = generateTrajectory(10, 1.0, 1.0); + EXPECT_DOUBLE_EQ(insertStopPoint(length, path).point.pose.position.x, 0.0); + } + { + const double length = 5.0; + auto path = generateTrajectory(10, 1.0, 1.0); + EXPECT_DOUBLE_EQ(insertStopPoint(length, path).point.pose.position.x, 5.0); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getSignedDistanceFromBoundary) +{ + using autoware::behavior_path_planner::utils::getSignedDistanceFromBoundary; + + { + lanelet::ConstLanelets empty_lanelets; + EXPECT_DOUBLE_EQ( + getSignedDistanceFromBoundary(empty_lanelets, planner_data_->self_odometry->pose.pose, true), + 0.0); + } + { + EXPECT_NEAR( + getSignedDistanceFromBoundary( + current_lanelets, planner_data_->self_odometry->pose.pose, true), + -1.4952926, epsilon); + EXPECT_NEAR( + getSignedDistanceFromBoundary( + current_lanelets, planner_data_->self_odometry->pose.pose, false), + 1.504715076, epsilon); + } + { + const double vehicle_width = 1.0; + const double base_link2front = 1.0; + const double base_link2rear = 1.0; + + const auto left_distance = getSignedDistanceFromBoundary( + current_lanelets, vehicle_width, base_link2front, base_link2rear, + planner_data_->self_odometry->pose.pose, true); + ASSERT_TRUE(left_distance.has_value()); + EXPECT_NEAR(left_distance.value(), -0.9946984, epsilon); + + const auto right_distance = getSignedDistanceFromBoundary( + current_lanelets, vehicle_width, base_link2front, base_link2rear, + planner_data_->self_odometry->pose.pose, false); + ASSERT_TRUE(right_distance.has_value()); + EXPECT_NEAR(right_distance.value(), 1.0041208, epsilon); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getArcLengthToTargetLanelet) +{ + using autoware::behavior_path_planner::utils::getArcLengthToTargetLanelet; + { + auto target_lane = current_lanelets.front(); + EXPECT_DOUBLE_EQ( + getArcLengthToTargetLanelet( + current_lanelets, target_lane, planner_data_->self_odometry->pose.pose), + 0.0); + } + { + auto target_lane = current_lanelets.at(1); + EXPECT_NEAR( + getArcLengthToTargetLanelet( + current_lanelets, target_lane, planner_data_->self_odometry->pose.pose), + 86.78265658, epsilon); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, toPolygon2d) +{ + using autoware::behavior_path_planner::utils::toPolygon2d; + + const auto lanelet = current_lanelets.front(); + const auto lanelet_polygon = lanelet.polygon2d().basicPolygon(); + + auto polygon_converted_from_lanelet = toPolygon2d(lanelet); + auto polygon_converted_from_basic_polygon = toPolygon2d(lanelet_polygon); + + EXPECT_EQ( + polygon_converted_from_lanelet.outer().size(), + polygon_converted_from_basic_polygon.outer().size()); + + for (size_t i = 0; i < polygon_converted_from_lanelet.outer().size(); i++) { + EXPECT_DOUBLE_EQ( + polygon_converted_from_lanelet.outer().at(i).x(), + polygon_converted_from_basic_polygon.outer().at(i).x()); + EXPECT_DOUBLE_EQ( + polygon_converted_from_lanelet.outer().at(i).y(), + polygon_converted_from_basic_polygon.outer().at(i).y()); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getHighestProbLabel) { using autoware::behavior_path_planner::utils::getHighestProbLabel; @@ -217,3 +468,87 @@ TEST(BehaviorPathPlanningUtilTest, getHighestProbLabel) .probability(0.6)); EXPECT_EQ(getHighestProbLabel(obj.classification), ObjectClassification::Type::TRUCK); } + +TEST_F(BehaviorPathPlanningUtilTest, getPrecedingLanelets) +{ + using autoware::behavior_path_planner::utils::getPrecedingLanelets; + const auto & route_handler_ptr = planner_data_->route_handler; + + { + const lanelet::ConstLanelets target_lanes; + EXPECT_TRUE(getPrecedingLanelets( + *route_handler_ptr, target_lanes, planner_data_->self_odometry->pose.pose, 10.0) + .empty()); + } + { + lanelet::ConstLanelets target_lanes; + target_lanes.push_back(route_handler_ptr->getLaneletsFromId(1001)); + EXPECT_TRUE(getPrecedingLanelets( + *route_handler_ptr, target_lanes, planner_data_->self_odometry->pose.pose, 0.0) + .empty()); + } + { + lanelet::ConstLanelets target_lanes; + target_lanes.push_back(route_handler_ptr->getLaneletsFromId(1011)); + target_lanes.push_back(route_handler_ptr->getLaneletsFromId(1101)); + const auto preceding_lanelets = getPrecedingLanelets( + *route_handler_ptr, target_lanes, planner_data_->self_odometry->pose.pose, 10.0); + ASSERT_FALSE(preceding_lanelets.empty()); + EXPECT_EQ(preceding_lanelets.front().data()->id(), 1001); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getBackwardLanelets) +{ + using autoware::behavior_path_planner::utils::getBackwardLanelets; + + const auto & route_handler_ptr = planner_data_->route_handler; + lanelet::ConstLanelets target_lanes; + target_lanes.push_back(route_handler_ptr->getLaneletsFromId(1011)); + const auto backward_lanelets = getBackwardLanelets( + *route_handler_ptr, target_lanes, planner_data_->self_odometry->pose.pose, 10.0); + ASSERT_FALSE(backward_lanelets.empty()); + EXPECT_EQ(backward_lanelets.front().id(), 1001); +} + +TEST_F(BehaviorPathPlanningUtilTest, calcLaneAroundPose) +{ + using autoware::behavior_path_planner::utils::calcLaneAroundPose; + + { + const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + std::shared_ptr route_handler_null = + std::make_shared(); + const auto lane = calcLaneAroundPose(route_handler_null, pose, 10.0, 0.0); + EXPECT_TRUE(lane.empty()); + } + { + const auto lane = calcLaneAroundPose( + planner_data_->route_handler, planner_data_->self_odometry->pose.pose, 10.0, 0.0); + EXPECT_EQ(lane.size(), 1); + EXPECT_EQ(lane.front().id(), 1001); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, checkPathRelativeAngle) +{ + using autoware::behavior_path_planner::utils::checkPathRelativeAngle; + + { + auto path = generateTrajectory(2, 1.0); + EXPECT_TRUE(checkPathRelativeAngle(path, 0.0)); + } + { + auto path = generateTrajectory(10, 1.0); + EXPECT_TRUE(checkPathRelativeAngle(path, M_PI_2)); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, convertToSnakeCase) +{ + using autoware::behavior_path_planner::utils::convertToSnakeCase; + + std::string input_string = "testString"; + auto converted_string = convertToSnakeCase(input_string); + EXPECT_EQ(converted_string, "test_string"); +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index ede912ea1c285..fac3cc92de1d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -14,6 +14,15 @@ #include "autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + namespace autoware::behavior_path_planner { using autoware::motion_utils::calcSignedArcLength; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/CMakeLists.txt index 520132dbd3363..334703e0edbb1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/CMakeLists.txt @@ -21,6 +21,21 @@ if(BUILD_TESTING) ) target_include_directories(test_${PROJECT_NAME} PRIVATE src) + + ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_utils + test/test_side_shift_utils.cpp + ) + + target_include_directories(test_${PROJECT_NAME}_utils PRIVATE src) + + ament_target_dependencies(test_${PROJECT_NAME}_utils + tf2 + tf2_geometry_msgs + ) + + target_link_libraries(test_${PROJECT_NAME}_utils + ${PROJECT_NAME} + ) endif() ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp index 543b17aca9352..15d81ff45abcf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp @@ -87,8 +87,6 @@ class SideShiftModule : public SceneModuleInterface // const methods void publishPath(const PathWithLaneId & path) const; - double getClosestShiftLength() const; - // member PathWithLaneId refined_path_{}; PathWithLaneId reference_path_{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp index d5888ab6a79d1..5a3e51c353c39 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp @@ -15,6 +15,8 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + #include #include #include @@ -27,9 +29,24 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using tier4_planning_msgs::msg::PathWithLaneId; +/** + * @brief Sets the orientation (yaw angle) for all points in the path. + * @param [in,out] path Path with lane ID to set orientation. + * @details For each point, calculates orientation based on: + * - Vector to next point if not last point + * - Vector from previous point if last point + * - Zero angle if single point + */ void setOrientation(PathWithLaneId * path); -bool isAlmostZero(double v); +/** + * @brief Gets the shift length at the closest path point to the ego position. + * @param [in] shifted_path Path with shift length information. + * @param [in] ego_point Current ego position. + * @return Shift length at the closest path point. Returns 0.0 if path is empty. + */ +double getClosestShiftLength( + const ShiftedPath & shifted_path, const geometry_msgs::msg::Point ego_point); } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index 5970a3a807517..22ee7ab77e077 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -26,6 +26,7 @@ #include #include #include +#include namespace autoware::behavior_path_planner { @@ -88,8 +89,7 @@ bool SideShiftModule::isExecutionRequested() const } // If the desired offset has a non-zero value, return true as we want to execute the plan. - - const bool has_request = !isAlmostZero(requested_lateral_offset_); + const bool has_request = std::fabs(requested_lateral_offset_) >= 1.0e-4; RCLCPP_DEBUG_STREAM( getLogger(), "ESS::isExecutionRequested() : " << std::boolalpha << has_request); @@ -119,6 +119,7 @@ bool SideShiftModule::canTransitSuccessState() { // Never return the FAILURE. When the desired offset is zero and the vehicle is in the original // drivable area,this module can stop the computation and return SUCCESS. + constexpr double ZERO_THRESHOLD = 1.0e-4; const auto isOffsetDiffAlmostZero = [this]() noexcept { const auto last_sp = path_shifter_.getLastShiftLine(); @@ -126,7 +127,7 @@ bool SideShiftModule::canTransitSuccessState() const auto length = std::fabs(last_sp.value().end_shift_length); const auto lateral_offset = std::fabs(requested_lateral_offset_); const auto offset_diff = lateral_offset - length; - if (!isAlmostZero(offset_diff)) { + if (std::fabs(offset_diff) >= ZERO_THRESHOLD) { lateral_offset_change_request_ = true; return false; } @@ -135,7 +136,7 @@ bool SideShiftModule::canTransitSuccessState() }(); const bool no_offset_diff = isOffsetDiffAlmostZero; - const bool no_request = isAlmostZero(requested_lateral_offset_); + const bool no_request = std::fabs(requested_lateral_offset_) < ZERO_THRESHOLD; const auto no_shifted_plan = [&]() { if (prev_output_.shift_length.empty()) { @@ -279,6 +280,11 @@ BehaviorModuleOutput SideShiftModule::plan() ShiftedPath shifted_path; path_shifter_.generate(&shifted_path); + if (shifted_path.path.points.empty()) { + RCLCPP_ERROR(getLogger(), "Generated shift_path has no points"); + return {}; + } + // Reset orientation setOrientation(&shifted_path.path); @@ -336,6 +342,7 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() return output; } +// can be moved to utils ShiftLine SideShiftModule::calcShiftLine() const { const auto & p = parameters_; @@ -345,7 +352,8 @@ ShiftLine SideShiftModule::calcShiftLine() const std::max(p->min_distance_to_start_shifting, ego_speed * p->time_to_start_shifting); const double dist_to_end = [&]() { - const double shift_length = requested_lateral_offset_ - getClosestShiftLength(); + const double shift_length = + requested_lateral_offset_ - getClosestShiftLength(prev_output_, getEgoPose().position); const double jerk_shifting_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, p->shifting_lateral_jerk, std::max(ego_speed, p->min_shifting_speed)); const double shifting_distance = std::max(jerk_shifting_distance, p->min_shifting_distance); @@ -367,18 +375,6 @@ ShiftLine SideShiftModule::calcShiftLine() const return shift_line; } -double SideShiftModule::getClosestShiftLength() const -{ - if (prev_output_.shift_length.empty()) { - return 0.0; - } - - const auto ego_point = planner_data_->self_odometry->pose.pose.position; - const auto closest = - autoware::motion_utils::findNearestIndex(prev_output_.path.points, ego_point); - return prev_output_.shift_length.at(closest); -} - BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & path) const { BehaviorModuleOutput out; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp index f30895c10d64d..942e87f57d05e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp @@ -26,12 +26,6 @@ namespace autoware::behavior_path_planner { void setOrientation(PathWithLaneId * path) { - if (!path) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("side_shift").get_child("util"), - "Pointer to path is NULL!"); - } - // Reset orientation for (size_t idx = 0; idx < path->points.size(); ++idx) { double angle = 0.0; @@ -53,9 +47,16 @@ void setOrientation(PathWithLaneId * path) } } -bool isAlmostZero(double v) +double getClosestShiftLength( + const ShiftedPath & shifted_path, const geometry_msgs::msg::Point ego_point) { - return std::fabs(v) < 1.0e-4; + if (shifted_path.shift_length.empty()) { + return 0.0; + } + + const auto closest = + autoware::motion_utils::findNearestIndex(shifted_path.path.points, ego_point); + return shifted_path.shift_length.at(closest); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index 1d70f1388d80b..c8661cdf1d912 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -21,6 +21,8 @@ #include #include +#include +#include #include using autoware::behavior_path_planner::BehaviorPathPlannerNode; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_side_shift_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_side_shift_utils.cpp new file mode 100644 index 0000000000000..b5bb54a6586ca --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_side_shift_utils.cpp @@ -0,0 +1,144 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "autoware/behavior_path_side_shift_module/utils.hpp" + +#include + +#include +#include +#include + +#include + +namespace autoware::behavior_path_planner +{ + +class SideShiftUtilsTest : public ::testing::Test +{ +protected: + PathWithLaneId generateStraightPath(const double length, const double width) + { + PathWithLaneId path; + const double interval = 1.0; + const size_t point_num = static_cast(length / interval); + + for (size_t i = 0; i < point_num; ++i) { + PathPointWithLaneId p; + p.point.pose.position.x = i * interval; + p.point.pose.position.y = width; + p.point.pose.position.z = 0.0; + + tf2::Quaternion q; + q.setRPY(0, 0, 0); + p.point.pose.orientation.x = q.x(); + p.point.pose.orientation.y = q.y(); + p.point.pose.orientation.z = q.z(); + p.point.pose.orientation.w = q.w(); + + path.points.push_back(p); + } + return path; + } + + ShiftedPath generateShiftedPath(const double length, const std::vector & shifts) + { + ShiftedPath shifted_path; + shifted_path.path = generateStraightPath(length, 0.0); + shifted_path.shift_length = shifts; + return shifted_path; + } +}; + +TEST_F(SideShiftUtilsTest, SetOrientationStraightPath) +{ + // Generate straight path + auto path = generateStraightPath(10.0, 0.0); + + // Set orientation + setOrientation(&path); + + // Check orientation for each point + for (const auto & p : path.points) { + double yaw = tf2::getYaw(p.point.pose.orientation); + EXPECT_NEAR(yaw, 0.0, 1e-6); // Should be facing forward (0 rad) + } +} + +TEST_F(SideShiftUtilsTest, SetOrientationCurvedPath) +{ + PathWithLaneId path; + + // Create a 90-degree turn path + PathPointWithLaneId p1, p2, p3; + + p1.point.pose.position.x = 0.0; + p1.point.pose.position.y = 0.0; + p1.point.pose.position.z = 0.0; + + p2.point.pose.position.x = 1.0; + p2.point.pose.position.y = 0.0; + p2.point.pose.position.z = 0.0; + + p3.point.pose.position.x = 1.0; + p3.point.pose.position.y = 1.0; + p3.point.pose.position.z = 0.0; + + path.points = {p1, p2, p3}; + + setOrientation(&path); + + // First segment should face east (0 rad) + EXPECT_NEAR(tf2::getYaw(path.points[0].point.pose.orientation), 0.0, 1e-6); + + // Last segment should face north (π/2 rad) + EXPECT_NEAR(tf2::getYaw(path.points[2].point.pose.orientation), M_PI_2, 1e-6); +} + +TEST_F(SideShiftUtilsTest, GetClosestShiftLengthEmptyPath) +{ + ShiftedPath empty_path; + geometry_msgs::msg::Point ego_point; + ego_point.x = 0.0; + ego_point.y = 0.0; + + double shift = getClosestShiftLength(empty_path, ego_point); + EXPECT_DOUBLE_EQ(shift, 0.0); +} + +TEST_F(SideShiftUtilsTest, GetClosestShiftLengthStraightPath) +{ + // Generate path with constant shift + std::vector shifts(10, 2.0); // 10 points with 2.0m shift + auto shifted_path = generateShiftedPath(10.0, shifts); + + // Test points at different positions + geometry_msgs::msg::Point ego_point; + + // Point at start + ego_point.x = 0.0; + ego_point.y = 0.0; + EXPECT_DOUBLE_EQ(getClosestShiftLength(shifted_path, ego_point), 2.0); + + // Point in middle + ego_point.x = 5.0; + ego_point.y = 0.0; + EXPECT_DOUBLE_EQ(getClosestShiftLength(shifted_path, ego_point), 2.0); + + // Point at end + ego_point.x = 9.0; + ego_point.y = 0.0; + EXPECT_DOUBLE_EQ(getClosestShiftLength(shifted_path, ego_point), 2.0); +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index c3f5e8883c284..42698f799c6b3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -21,6 +21,8 @@ #include +#include +#include #include #include @@ -38,12 +40,13 @@ FreespacePullOut::FreespacePullOut( if (parameters.freespace_planner_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; planner_ = std::make_unique( - parameters.freespace_planner_common_parameters, vehicle_shape, parameters.astar_parameters); + parameters.freespace_planner_common_parameters, vehicle_shape, parameters.astar_parameters, + node.get_clock()); } else if (parameters.freespace_planner_algorithm == "rrtstar") { use_back_ = true; // no option for disabling back in rrtstar planner_ = std::make_unique( - parameters.freespace_planner_common_parameters, vehicle_shape, - parameters.rrt_star_parameters); + parameters.freespace_planner_common_parameters, vehicle_shape, parameters.rrt_star_parameters, + node.get_clock()); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 13a9908b97daf..bbc6c05725434 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -22,6 +22,10 @@ #include +#include +#include +#include + using autoware::motion_utils::findNearestIndex; using autoware::universe_utils::calcDistance2d; using autoware::universe_utils::calcOffsetPose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index aa3246eb3dac0..4759559619870 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -25,7 +25,10 @@ #include #include +#include +#include #include +#include #include using autoware::motion_utils::findNearestIndex; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 7dd597e27b71f..20cac638cebbd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -34,9 +34,13 @@ #include #include +#include +#include +#include #include #include #include +#include #include #include @@ -1372,7 +1376,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() path.points, status_.pull_out_path.start_pose.position); const auto shift_end_idx = autoware::motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position); - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(planner_data_); const auto is_ignore_signal = [this](const lanelet::Id & id) { if (!ignore_signal_.has_value()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index 0a990ddf4cb52..ca7a2dab1191c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -14,6 +14,7 @@ Shumpei Wakabayashi Tomohito Ando Go Sakayori + Yukinari Hisaki Apache License 2.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 997fb7353b14c..e7177768be3dd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -20,6 +20,8 @@ #include #include +#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index f7fc4ee48d364..76bfa621ac0e4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 917736b1680db..744af35641a59 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -17,6 +17,11 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include +#include +#include +#include + namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index cc036b687b105..c3d494c8ffe5d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 8cf5696a89372..b6a8fd04f93db 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include using autoware::behavior_path_planner::BehaviorPathPlannerNode; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp index 7b0cef7e00bb6..6911be800bdc2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp @@ -26,6 +26,9 @@ #include #include +#include +#include + namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt index bdaa6d9f6aa2f..b4a688d711221 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/manager.cpp src/scene.cpp src/decisions.cpp + src/util.cpp ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp similarity index 85% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp index edc4ea502b396..f296ad03cbac6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MANAGER_HPP_ -#define MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ -#include "scene.hpp" +#include "autoware/behavior_velocity_blind_spot_module/scene.hpp" #include #include @@ -52,4 +52,4 @@ class BlindSpotModulePlugin : public PluginWrapper } // namespace autoware::behavior_velocity_planner -#endif // MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp similarity index 70% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 0c913964332ab..1bfa41d86ffbe 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -12,20 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_HPP_ -#define SCENE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ +#include #include -#include #include #include -#include #include -#include -#include -#include #include #include @@ -37,21 +33,6 @@ namespace autoware::behavior_velocity_planner { -/** - * @brief wrapper class of interpolated path with lane id - */ -struct InterpolatedPathInfo -{ - /** the interpolated path */ - tier4_planning_msgs::msg::PathWithLaneId path; - /** discretization interval of interpolation */ - double ds{0.0}; - /** the intersection lanelet id */ - lanelet::Id lane_id{0}; - /** the range of indices for the path points with associative lane id */ - std::optional> lane_id_interval{std::nullopt}; -}; - /** * @brief represent action */ @@ -81,8 +62,6 @@ using BlindSpotDecision = std::variant virtual_wall_pose{std::nullopt}; @@ -114,7 +93,7 @@ class BlindSpotModule : public SceneModuleInterface * @brief plan go-stop velocity at traffic crossing with collision check between reference path * and object predicted path */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; std::vector createVirtualWalls() override; @@ -133,7 +112,7 @@ class BlindSpotModule : public SceneModuleInterface // Parameter void initializeRTCStatus(); - BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path); // setSafe(), setDistance() void setRTCStatus( const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); @@ -141,18 +120,10 @@ class BlindSpotModule : public SceneModuleInterface void setRTCStatusByDecision( const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); // stop/GO - void reactRTCApproval( - const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason); + void reactRTCApproval(const BlindSpotDecision & decision, PathWithLaneId * path); template void reactRTCApprovalByDecision( - const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason); - - std::optional generateInterpolatedPathInfo( - const tier4_planning_msgs::msg::PathWithLaneId & input_path) const; - - std::optional getSiblingStraightLanelet( - const std::shared_ptr planner_data) const; + const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path); /** * @brief Generate a stop line and insert it into the path. @@ -190,33 +161,6 @@ class BlindSpotModule : public SceneModuleInterface const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, const double ego_time_to_reach_stop_line); - /** - * @brief Create half lanelet - * @param lanelet input lanelet - * @return Half lanelet - */ - lanelet::ConstLanelet generateHalfLanelet(const lanelet::ConstLanelet lanelet) const; - - lanelet::ConstLanelet generateExtendedAdjacentLanelet( - const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; - 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 - * @param path path information associated with lane id - * @param closest_idx closest path point index from ego car in path points - * @return Blind spot polygons - */ - 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 & stop_line_pose) const; - /** * @brief Check if object is belong to targeted classes * @param object Dynamic object @@ -241,4 +185,4 @@ class BlindSpotModule : public SceneModuleInterface }; } // namespace autoware::behavior_velocity_planner -#endif // SCENE_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp new file mode 100644 index 0000000000000..9d908414b6d95 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp @@ -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. + +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__UTIL_HPP_ + +#include + +#include + +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +enum class TurnDirection { LEFT, RIGHT }; + +/** + * @brief wrapper class of interpolated path with lane id + */ +struct InterpolatedPathInfo +{ + /** the interpolated path */ + tier4_planning_msgs::msg::PathWithLaneId path; + /** discretization interval of interpolation */ + double ds{0.0}; + /** the intersection lanelet id */ + lanelet::Id lane_id{0}; + /** the range of indices for the path points with associative lane id */ + std::optional> lane_id_interval{std::nullopt}; +}; + +std::optional generateInterpolatedPathInfo( + const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, + rclcpp::Logger logger); + +std::optional getSiblingStraightLanelet( + const lanelet::Lanelet assigned_lane, + const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr); + +/** + * @brief Create half lanelet + * @param lanelet input lanelet + * @return Half lanelet + */ +lanelet::ConstLanelet generateHalfLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection & turn_direction, + const double ignore_width_from_centerline); + +lanelet::ConstLanelet generateExtendedAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction, + const double adjacent_extend_width); +lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction, + const double opposite_adjacent_extend_width); + +lanelet::ConstLanelets generateBlindSpotLanelets( + const std::shared_ptr route_handler, + const TurnDirection turn_direction, const lanelet::Id lane_id, + const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, + const double adjacent_extend_width, const double opposite_adjacent_extend_width); + +/** + * @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 + * @param path path information associated with lane id + * @param closest_idx closest path point index from ego car in path points + * @return Blind spot polygons + */ +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 & stop_line_pose, const double backward_detection_length); + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index de5d791c06161..b484c6a66cbac 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -8,6 +8,7 @@ Mamoru Sobue Tomoya Kimura Shumpei Wakabayashi + Yukinari Hisaki Apache License 2.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp index 714280daad38e..2a6f4825ba414 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene.hpp" +#include "autoware/behavior_velocity_blind_spot_module/scene.hpp" #include #include @@ -24,6 +24,7 @@ #include #include +#include namespace autoware::behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 116050871c722..64e1435167a89 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene.hpp" +#include "autoware/behavior_velocity_blind_spot_module/scene.hpp" #include @@ -33,8 +33,7 @@ void BlindSpotModule::setRTCStatusByDecision( template void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const T & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] StopReason * stop_reason) + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) { static_assert("Unsupported type passed to reactRTCApprovalByDecision"); } @@ -53,8 +52,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const InternalError & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] StopReason * stop_reason) + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) { return; } @@ -73,8 +71,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const OverPassJudge & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] StopReason * stop_reason) + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) { return; } @@ -95,8 +92,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason) + const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path) { if (!isActivated()) { constexpr double stop_vel = 0.0; @@ -104,11 +100,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( 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); } @@ -131,7 +123,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) + const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path) { if (!isActivated()) { constexpr double stop_vel = 0.0; @@ -139,11 +131,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( 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); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index 4e6cebeef2162..cc63b42df68e4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "manager.hpp" +#include "autoware/behavior_velocity_blind_spot_module/manager.hpp" + +#include "autoware/behavior_velocity_blind_spot_module/util.hpp" #include #include @@ -23,7 +25,6 @@ #include #include #include -#include namespace autoware::behavior_velocity_planner { @@ -69,9 +70,8 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa if (turn_direction_str != "left" && turn_direction_str != "right") { continue; } - const auto turn_direction = turn_direction_str == "left" - ? BlindSpotModule::TurnDirection::LEFT - : BlindSpotModule::TurnDirection::RIGHT; + const auto turn_direction = + turn_direction_str == "left" ? TurnDirection::LEFT : TurnDirection::RIGHT; registerModule(std::make_shared( module_id, lane_id, turn_direction, planner_data_, planner_param_, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 2eb54f91a3e26..7697786adb19d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -12,31 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene.hpp" +#include "autoware/behavior_velocity_blind_spot_module/scene.hpp" + +#include "autoware/behavior_velocity_blind_spot_module/util.hpp" #include -#include #include #include -#include -#include #include -#include -#include -#include -#include +#include -#include #include -#include #include #include #include #include #include -#include #include #include @@ -55,7 +48,9 @@ BlindSpotModule::BlindSpotModule( is_over_pass_judge_line_(false) { velocity_factor_.init(PlanningBehavior::REAR_CHECK); - sibling_straight_lanelet_ = getSiblingStraightLanelet(planner_data); + sibling_straight_lanelet_ = getSiblingStraightLanelet( + planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_), + planner_data->route_handler_->getRoutingGraphPtr()); } void BlindSpotModule::initializeRTCStatus() @@ -64,8 +59,7 @@ void BlindSpotModule::initializeRTCStatus() setDistance(std::numeric_limits::lowest()); } -BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(PathWithLaneId * path) { if (planner_param_.use_pass_judge_line && is_over_pass_judge_line_) { return OverPassJudge{"already over the pass judge line. no plan needed."}; @@ -73,7 +67,8 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( const auto & input_path = *path; /* set stop-line and stop-judgement-line for base_link */ - const auto interpolated_path_info_opt = generateInterpolatedPathInfo(input_path); + const auto interpolated_path_info_opt = + generateInterpolatedPathInfo(lane_id_, input_path, logger_); if (!interpolated_path_info_opt) { return InternalError{"Failed to interpolate path"}; } @@ -106,7 +101,10 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( } if (!blind_spot_lanelets_) { - const auto blind_spot_lanelets = generateBlindSpotLanelets(input_path); + const auto blind_spot_lanelets = generateBlindSpotLanelets( + planner_data_->route_handler_, turn_direction_, lane_id_, input_path, + planner_param_.ignore_width_from_center_line, planner_param_.adjacent_extend_width, + planner_param_.opposite_adjacent_extend_width); if (!blind_spot_lanelets.empty()) { blind_spot_lanelets_ = blind_spot_lanelets; } @@ -117,8 +115,8 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( const auto & blind_spot_lanelets = blind_spot_lanelets_.value(); const auto detection_area_opt = generateBlindSpotPolygons( - input_path, closest_idx, blind_spot_lanelets, - path->points.at(critical_stopline_idx).point.pose); + input_path, closest_idx, blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose, + planner_param_.backward_detection_length); if (!detection_area_opt) { return InternalError{"Failed to generate BlindSpotPolygons"}; } @@ -160,100 +158,27 @@ void BlindSpotModule::setRTCStatus( decision); } -void BlindSpotModule::reactRTCApproval( - const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason) +void BlindSpotModule::reactRTCApproval(const BlindSpotDecision & decision, PathWithLaneId * path) { std::visit( - VisitorSwitch{[&](const auto & sub_decision) { - reactRTCApprovalByDecision(sub_decision, path, stop_reason); - }}, + VisitorSwitch{ + [&](const auto & sub_decision) { reactRTCApprovalByDecision(sub_decision, path); }}, decision); } -bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path) { debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); initializeRTCStatus(); - const auto decision = modifyPathVelocityDetail(path, stop_reason); + const auto decision = modifyPathVelocityDetail(path); const auto & input_path = *path; setRTCStatus(decision, input_path); - reactRTCApproval(decision, path, stop_reason); + reactRTCApproval(decision, path); return true; } -static bool hasLaneIds( - const tier4_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) -{ - for (const auto & pid : p.lane_ids) { - if (pid == id) { - return true; - } - } - return false; -} - -static std::optional> findLaneIdInterval( - const tier4_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) -{ - bool found = false; - size_t start = 0; - size_t end = p.points.size() > 0 ? p.points.size() - 1 : 0; - if (start == end) { - // there is only one point in the path - return std::nullopt; - } - for (size_t i = 0; i < p.points.size(); ++i) { - if (hasLaneIds(p.points.at(i), id)) { - if (!found) { - // found interval for the first time - found = true; - start = i; - } - } else if (found) { - // prior point was in the interval. interval ended - end = i; - break; - } - } - start = start > 0 ? start - 1 : 0; // the idx of last point before the interval - return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt; -} - -std::optional BlindSpotModule::generateInterpolatedPathInfo( - const tier4_planning_msgs::msg::PathWithLaneId & input_path) const -{ - constexpr double ds = 0.2; - InterpolatedPathInfo interpolated_path_info; - if (!splineInterpolate(input_path, ds, interpolated_path_info.path, logger_)) { - return std::nullopt; - } - interpolated_path_info.ds = ds; - interpolated_path_info.lane_id = lane_id_; - interpolated_path_info.lane_id_interval = - findLaneIdInterval(interpolated_path_info.path, lane_id_); - return interpolated_path_info; -} - -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 assigned_lane = lanelet_map_ptr->laneletLayer.get(lane_id_); - for (const auto & prev : routing_graph_ptr->previous(assigned_lane)) { - for (const auto & following : routing_graph_ptr->following(prev)) { - if (std::string(following.attributeOr("turn_direction", "else")) == "straight") { - return following; - } - } - } - return std::nullopt; -} - static std::optional getFirstPointIntersectsLineByFootprint( const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) @@ -445,7 +370,7 @@ double BlindSpotModule::computeTimeToPassStopLine( const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose) const { - // egoが停止している時にそのまま速度を使うと衝突しなくなってしまうのでegoについては最低速度を使う + // if ego is completely stopped, using ego velocity yields "no-collision" const auto & current_pose = planner_data_->current_odometry->pose; const auto current_arc_ego = lanelet::utils::getArcCoordinates(blind_spot_lanelets, current_pose).length; @@ -495,220 +420,6 @@ std::optional BlindSpotModule::i return std::nullopt; } -lanelet::ConstLanelet BlindSpotModule::generateHalfLanelet( - const lanelet::ConstLanelet lanelet) const -{ - lanelet::Points3d lefts, rights; - - const double offset = (turn_direction_ == TurnDirection::LEFT) - ? planner_param_.ignore_width_from_center_line - : -planner_param_.ignore_width_from_center_line; - const auto offset_centerline = lanelet::utils::getCenterlineWithOffset(lanelet, offset); - - const auto original_left_bound = - (turn_direction_ == TurnDirection::LEFT) ? lanelet.leftBound() : offset_centerline; - const auto original_right_bound = - (turn_direction_ == TurnDirection::LEFT) ? offset_centerline : lanelet.rightBound(); - - for (const auto & pt : original_left_bound) { - lefts.push_back(lanelet::Point3d(pt)); - } - for (const auto & pt : original_right_bound) { - rights.push_back(lanelet::Point3d(pt)); - } - 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); - return half_lanelet; -} - -lanelet::ConstLanelet BlindSpotModule::generateExtendedAdjacentLanelet( - const lanelet::ConstLanelet lanelet, const TurnDirection direction) const -{ - const auto centerline = lanelet.centerline2d(); - const auto width = - boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline); - const double extend_width = std::min(planner_param_.adjacent_extend_width, width); - const auto left_bound_ = - direction == TurnDirection::LEFT - ? lanelet::utils::getCenterlineWithOffset(lanelet, -width / 2 + extend_width) - : lanelet.leftBound(); - const auto right_bound_ = - direction == TurnDirection::RIGHT - ? lanelet::utils::getCenterlineWithOffset(lanelet, width / 2 - extend_width) - : lanelet.rightBound(); - lanelet::Points3d lefts, rights; - for (const auto & pt : left_bound_) { - lefts.push_back(lanelet::Point3d(pt)); - } - for (const auto & pt : right_bound_) { - rights.push_back(lanelet::Point3d(pt)); - } - const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); - const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); - auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); - const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0); - new_lanelet.setCenterline(new_centerline); - return new_lanelet; -} - -lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet( - const lanelet::ConstLanelet lanelet, const TurnDirection direction) const -{ - const auto centerline = lanelet.centerline2d(); - const auto width = - boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline); - const double extend_width = - std::min(planner_param_.opposite_adjacent_extend_width, width); - const auto left_bound_ = - direction == TurnDirection::RIGHT - ? lanelet.rightBound().invert() - : lanelet::utils::getCenterlineWithOffset(lanelet.invert(), -width / 2 + extend_width); - const auto right_bound_ = - direction == TurnDirection::RIGHT - ? lanelet::utils::getCenterlineWithOffset(lanelet.invert(), width / 2 - extend_width) - : lanelet.leftBound().invert(); - lanelet::Points3d lefts, rights; - for (const auto & pt : left_bound_) { - lefts.push_back(lanelet::Point3d(pt)); - } - for (const auto & pt : right_bound_) { - rights.push_back(lanelet::Point3d(pt)); - } - const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); - const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); - auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); - const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0); - new_lanelet.setCenterline(new_centerline); - return new_lanelet; -} - -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; - /* get lane ids until intersection */ - for (const auto & point : path.points) { - bool found_intersection_lane = false; - for (const auto lane_id : point.lane_ids) { - if (lane_id == lane_id_) { - found_intersection_lane = true; - lane_ids.push_back(lane_id); - break; - } - // make lane_ids unique - if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { - lane_ids.push_back(lane_id); - } - } - if (found_intersection_lane) break; - } - - 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 assoc_adj = - turn_direction_ == TurnDirection::LEFT - ? (routing_graph_ptr->adjacentLeft(lane)) - : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) - : boost::none); - const std::optional opposite_adj = - [&]() -> std::optional { - if (!!assoc_adj) { - return std::nullopt; - } - if (turn_direction_ == TurnDirection::LEFT) { - // this should exist in right-hand traffic - const auto adjacent_lanes = - lanelet_map_ptr->laneletLayer.findUsages(lane.leftBound().invert()); - if (adjacent_lanes.empty()) { - return std::nullopt; - } - return adjacent_lanes.front(); - } - if (turn_direction_ == TurnDirection::RIGHT) { - // this should exist in left-hand traffic - const auto adjacent_lanes = - lanelet_map_ptr->laneletLayer.findUsages(lane.rightBound().invert()); - if (adjacent_lanes.empty()) { - return std::nullopt; - } - return adjacent_lanes.front(); - } else { - return std::nullopt; - } - }(); - - const auto assoc_shoulder = [&]() -> std::optional { - if (turn_direction_ == TurnDirection::LEFT) { - return planner_data_->route_handler_->getLeftShoulderLanelet(lane); - } else if (turn_direction_ == TurnDirection::RIGHT) { - return planner_data_->route_handler_->getRightShoulderLanelet(lane); - } - return std::nullopt; - }(); - if (assoc_shoulder) { - const auto lefts = (turn_direction_ == TurnDirection::LEFT) - ? assoc_shoulder.value().leftBound() - : ego_half_lanelet.leftBound(); - const auto rights = (turn_direction_ == TurnDirection::LEFT) - ? ego_half_lanelet.rightBound() - : assoc_shoulder.value().rightBound(); - blind_spot_lanelets.push_back( - lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); - - } else if (!!assoc_adj) { - const auto adj_half_lanelet = - generateExtendedAdjacentLanelet(assoc_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_); - 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))); - } else { - blind_spot_lanelets.push_back(ego_half_lanelet); - } - } - return blind_spot_lanelets; -} - -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 = - 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_perception_msgs::msg::PredictedObject & object) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp new file mode 100644 index 0000000000000..5c5aa0a26b3b4 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp @@ -0,0 +1,320 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +namespace +{ +static bool hasLaneIds( + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) +{ + for (const auto & pid : p.lane_ids) { + if (pid == id) { + return true; + } + } + return false; +} + +static std::optional> findLaneIdInterval( + const tier4_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) +{ + bool found = false; + size_t start = 0; + size_t end = p.points.size() > 0 ? p.points.size() - 1 : 0; + if (start == end) { + // there is only one point in the path + return std::nullopt; + } + for (size_t i = 0; i < p.points.size(); ++i) { + if (hasLaneIds(p.points.at(i), id)) { + if (!found) { + // found interval for the first time + found = true; + start = i; + } + } else if (found) { + // prior point was in the interval. interval ended + end = i; + break; + } + } + start = start > 0 ? start - 1 : 0; // the idx of last point before the interval + return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt; +} +} // namespace + +std::optional generateInterpolatedPathInfo( + const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, + rclcpp::Logger logger) +{ + constexpr double ds = 0.2; + InterpolatedPathInfo interpolated_path_info; + if (!splineInterpolate(input_path, ds, interpolated_path_info.path, logger)) { + return std::nullopt; + } + interpolated_path_info.ds = ds; + interpolated_path_info.lane_id = lane_id; + interpolated_path_info.lane_id_interval = + findLaneIdInterval(interpolated_path_info.path, lane_id); + return interpolated_path_info; +} + +std::optional getSiblingStraightLanelet( + const lanelet::Lanelet assigned_lane, + const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr) +{ + for (const auto & prev : routing_graph_ptr->previous(assigned_lane)) { + for (const auto & following : routing_graph_ptr->following(prev)) { + if (std::string(following.attributeOr("turn_direction", "else")) == "straight") { + return following; + } + } + } + return std::nullopt; +} + +lanelet::ConstLanelet generateHalfLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection & turn_direction, + const double ignore_width_from_centerline) +{ + lanelet::Points3d lefts, rights; + + const double offset = (turn_direction == TurnDirection::LEFT) ? ignore_width_from_centerline + : -ignore_width_from_centerline; + const auto offset_centerline = lanelet::utils::getCenterlineWithOffset(lanelet, offset); + + const auto original_left_bound = + (turn_direction == TurnDirection::LEFT) ? lanelet.leftBound() : offset_centerline; + const auto original_right_bound = + (turn_direction == TurnDirection::LEFT) ? offset_centerline : lanelet.rightBound(); + + for (const auto & pt : original_left_bound) { + lefts.emplace_back(pt); + } + for (const auto & pt : original_right_bound) { + rights.emplace_back(pt); + } + 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); + return half_lanelet; +} + +lanelet::ConstLanelet generateExtendedAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction, + const double adjacent_extend_width) +{ + const auto centerline = lanelet.centerline2d(); + const auto width = + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline); + const double extend_width = std::min(adjacent_extend_width, width); + const auto left_bound_ = + direction == TurnDirection::LEFT + ? lanelet::utils::getCenterlineWithOffset(lanelet, -width / 2 + extend_width) + : lanelet.leftBound(); + const auto right_bound_ = + direction == TurnDirection::RIGHT + ? lanelet::utils::getCenterlineWithOffset(lanelet, width / 2 - extend_width) + : lanelet.rightBound(); + lanelet::Points3d lefts, rights; + for (const auto & pt : left_bound_) { + lefts.emplace_back(pt); + } + for (const auto & pt : right_bound_) { + rights.emplace_back(pt); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); + auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0); + new_lanelet.setCenterline(new_centerline); + return new_lanelet; +} + +lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction, + const double opposite_adjacent_extend_width) +{ + const auto centerline = lanelet.centerline2d(); + const auto width = + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline); + const double extend_width = std::min(opposite_adjacent_extend_width, width); + const auto left_bound_ = + direction == TurnDirection::RIGHT + ? lanelet.rightBound().invert() + : lanelet::utils::getCenterlineWithOffset(lanelet.invert(), -width / 2 + extend_width); + const auto right_bound_ = + direction == TurnDirection::RIGHT + ? lanelet::utils::getCenterlineWithOffset(lanelet.invert(), width / 2 - extend_width) + : lanelet.leftBound().invert(); + lanelet::Points3d lefts, rights; + for (const auto & pt : left_bound_) { + lefts.emplace_back(pt); + } + for (const auto & pt : right_bound_) { + rights.emplace_back(pt); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); + auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0); + new_lanelet.setCenterline(new_centerline); + return new_lanelet; +} + +static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) +{ + lanelet::Points3d pts; + for (const auto & pt : line) { + pts.emplace_back(pt); + } + return lanelet::LineString3d(lanelet::InvalId, pts); +} + +lanelet::ConstLanelets generateBlindSpotLanelets( + const std::shared_ptr route_handler, + const TurnDirection turn_direction, const lanelet::Id lane_id, + const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, + const double adjacent_extend_width, const double opposite_adjacent_extend_width) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); + + std::vector lane_ids; + /* get lane ids until intersection */ + for (const auto & point : path.points) { + bool found_intersection_lane = false; + for (const auto id : point.lane_ids) { + if (id == lane_id) { + found_intersection_lane = true; + lane_ids.push_back(lane_id); + break; + } + // make lane_ids unique + if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { + lane_ids.push_back(lane_id); + } + } + if (found_intersection_lane) break; + } + + 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, turn_direction, ignore_width_from_centerline); + const auto assoc_adj = + turn_direction == TurnDirection::LEFT + ? (routing_graph_ptr->adjacentLeft(lane)) + : (turn_direction == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) + : boost::none); + const std::optional opposite_adj = + [&]() -> std::optional { + if (!!assoc_adj) { + return std::nullopt; + } + if (turn_direction == TurnDirection::LEFT) { + // this should exist in right-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.leftBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); + } + if (turn_direction == TurnDirection::RIGHT) { + // this should exist in left-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.rightBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); + } else { + return std::nullopt; + } + }(); + + const auto assoc_shoulder = [&]() -> std::optional { + if (turn_direction == TurnDirection::LEFT) { + return route_handler->getLeftShoulderLanelet(lane); + } else if (turn_direction == TurnDirection::RIGHT) { + return route_handler->getRightShoulderLanelet(lane); + } + return std::nullopt; + }(); + if (assoc_shoulder) { + const auto lefts = (turn_direction == TurnDirection::LEFT) + ? assoc_shoulder.value().leftBound() + : ego_half_lanelet.leftBound(); + const auto rights = (turn_direction == TurnDirection::LEFT) + ? ego_half_lanelet.rightBound() + : assoc_shoulder.value().rightBound(); + blind_spot_lanelets.emplace_back(lanelet::InvalId, removeConst(lefts), removeConst(rights)); + + } else if (!!assoc_adj) { + const auto adj_half_lanelet = + generateExtendedAdjacentLanelet(assoc_adj.value(), turn_direction, adjacent_extend_width); + 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.emplace_back(lanelet::InvalId, removeConst(lefts), removeConst(rights)); + } else if (opposite_adj) { + const auto adj_half_lanelet = generateExtendedOppositeAdjacentLanelet( + opposite_adj.value(), turn_direction, opposite_adjacent_extend_width); + 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.emplace_back(lanelet::InvalId, removeConst(lefts), removeConst(rights)); + } else { + blind_spot_lanelets.push_back(ego_half_lanelet); + } + } + return blind_spot_lanelets; +} + +std::optional 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 double backward_detection_length) +{ + const auto stop_line_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length; + const auto detection_area_start_length_ego = + std::max(stop_line_arc_ego - backward_detection_length, 0.0); + return lanelet::utils::getPolygonFromArcLength( + blind_spot_lanelets, detection_area_start_length_ego, stop_line_arc_ego); +} + +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 51a0b4aa23fde..1bea0626b0b2f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -35,7 +35,9 @@ #include #include #include +#include #include +#include #include namespace autoware::behavior_velocity_planner @@ -201,7 +203,7 @@ CrosswalkModule::CrosswalkModule( node.create_publisher("~/debug/collision_info", 1); } -bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path) { if (path->points.size() < 2) { RCLCPP_DEBUG(logger_, "Do not interpolate because path size is less than 2."); @@ -213,7 +215,6 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto logger_, planner_param_.show_processing_time, "=========== module_id: %ld ===========", module_id_); - *stop_reason = planning_utils::initializeStopReason(StopReason::CROSSWALK); const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto objects_ptr = planner_data_->predicted_objects; @@ -274,7 +275,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto if (isActivated()) { planGo(*path, nearest_stop_factor); } else { - planStop(*path, nearest_stop_factor, default_stop_pose, stop_reason); + planStop(*path, nearest_stop_factor, default_stop_pose); } recordTime(4); @@ -1353,7 +1354,7 @@ void CrosswalkModule::planGo( void CrosswalkModule::planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, - const std::optional & default_stop_pose, StopReason * stop_reason) + const std::optional & default_stop_pose) { // Calculate stop factor auto stop_factor = [&]() -> std::optional { @@ -1376,7 +1377,6 @@ void CrosswalkModule::planStop( // Plan stop insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path); - planning_utils::appendStopReason(*stop_factor, stop_reason); velocity_factor_.set( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index de696aa670ad3..8577ed1669b48 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -336,7 +337,7 @@ class CrosswalkModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; @@ -399,7 +400,7 @@ class CrosswalkModule : public SceneModuleInterface void planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, - const std::optional & default_stop_pose, StopReason * stop_reason); + const std::optional & default_stop_pose); // minor functions std::pair getAttentionRange( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp index a72c866a85cef..50de1b1a8e1ff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_crosswalk.cpp index b626ef1772853..8af879578ed89 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_crosswalk.cpp @@ -16,6 +16,8 @@ #include +#include + TEST(CrosswalkTest, CheckInterpolateEgoPassMargin) { namespace bvp = autoware::behavior_velocity_planner; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index abbb818f2b042..61b3b185999d8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -47,7 +47,7 @@ DetectionAreaModule::DetectionAreaModule( velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } -bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) { // Store original path const auto original_path = *path; @@ -55,7 +55,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Reset data debug_data_ = DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - *stop_reason = planning_utils::initializeStopReason(StopReason::DETECTION_AREA); // Find obstacles in detection area const auto obstacle_points = detection_area::get_obstacle_points( @@ -184,28 +183,11 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Create StopReason { - StopFactor stop_factor{}; - stop_factor.stop_pose = stop_point->second; - stop_factor.stop_factor_points = obstacle_points; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_point->second, VelocityFactor::UNKNOWN); } - // Create legacy StopReason - { - const double stop_path_point_distance = autoware::motion_utils::calcSignedArcLength( - path->points, 0, stop_pose.position, stop_point->first); - - if ( - !first_stop_path_point_distance_ || - stop_path_point_distance < first_stop_path_point_distance_.value()) { - debug_data_.first_stop_pose = stop_point->second; - first_stop_path_point_distance_ = stop_path_point_distance; - } - } - return true; } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 46bb46bcdeb61..9224cf4624687 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -70,7 +70,7 @@ class DetectionAreaModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp index 6b3bf416e6d99..0d721fc81bcee 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp @@ -22,7 +22,9 @@ #include #include +#include #include +#include namespace { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp index ffffa4da8f4a1..e50bc041cbc89 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp @@ -22,6 +22,8 @@ #include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/decision_result.cpp index c211e1f43783e..bfdc36afa9011 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/decision_result.cpp @@ -14,6 +14,8 @@ #include "decision_result.hpp" +#include + namespace autoware::behavior_velocity_planner { std::string formatDecisionResult( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index d93fbf271aa1d..9fd5ba4ab7d5c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -25,6 +25,8 @@ #include #include +#include +#include #include namespace diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 8115f52437a04..6f7841ebb0bbb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -36,6 +36,9 @@ #include #include #include +#include +#include +#include #include namespace autoware::behavior_velocity_planner @@ -91,14 +94,13 @@ IntersectionModule::IntersectionModule( "~/debug/intersection/object_ttc", 1); } -bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path) { debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); initializeRTCStatus(); - const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + const auto decision_result = modifyPathVelocityDetail(path); prev_decision_result_ = decision_result; { @@ -110,7 +112,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * prepareRTCStatus(decision_result, *path); - reactRTCApproval(decision_result, path, stop_reason); + reactRTCApproval(decision_result, path); return true; } @@ -145,8 +147,7 @@ static std::string formatOcclusionType(const IntersectionModule::OcclusionType & return "RTCOccluded"; } -DecisionResult IntersectionModule::modifyPathVelocityDetail( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * path) { const auto prepare_data = prepareIntersectionData(path); if (!prepare_data) { @@ -683,7 +684,6 @@ void prepareRTCByDecisionResult( *occlusion_safety = true; *occlusion_distance = autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); - return; } void IntersectionModule::prepareRTCStatus( @@ -711,7 +711,6 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { @@ -727,7 +726,6 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[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 +740,6 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[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,8 +752,7 @@ void reactRTCApprovalByDecisionResult( const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -770,10 +766,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -785,9 +777,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(occlusion_stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, path->points.at(occlusion_stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -802,8 +791,7 @@ void reactRTCApprovalByDecisionResult( const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -817,10 +805,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -835,8 +819,7 @@ void reactRTCApprovalByDecisionResult( const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -848,9 +831,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -862,9 +842,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -878,8 +855,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -891,9 +868,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_first_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -913,9 +887,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -929,8 +900,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -955,9 +926,6 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(occlusion_peeking_stopline).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(occlusion_peeking_stopline).point.pose, VelocityFactor::UNKNOWN); @@ -969,9 +937,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -986,8 +951,7 @@ void reactRTCApprovalByDecisionResult( const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -999,9 +963,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1017,9 +978,6 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1034,8 +992,7 @@ void reactRTCApprovalByDecisionResult( const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1047,9 +1004,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1061,9 +1015,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1087,8 +1038,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1099,9 +1049,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1113,9 +1060,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1130,8 +1074,7 @@ void reactRTCApprovalByDecisionResult( const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, - IntersectionModule::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1143,9 +1086,6 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1157,9 +1097,6 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); @@ -1169,15 +1106,14 @@ void reactRTCApprovalByDecisionResult( } void IntersectionModule::reactRTCApproval( - const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason) + const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path) { const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( VisitorSwitch{[&](const auto & decision) { reactRTCApprovalByDecisionResult( activated_, occlusion_activated_, decision, planner_param_, baselink2front, path, - stop_reason, &velocity_factor_, &debug_data_); + &velocity_factor_, &debug_data_); }}, decision_result); return; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index 8ffb4792f55db..b718dd84d33af 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -321,7 +321,7 @@ class IntersectionModule : public SceneModuleInterface * INTERSECTION_OCCLUSION. * @{ */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; /** @}*/ visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; @@ -504,7 +504,7 @@ class IntersectionModule : public SceneModuleInterface /** * @brief analyze traffic_light/occupancy/objects context and return DecisionResult */ - DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + DecisionResult modifyPathVelocityDetail(PathWithLaneId * path); /** * @brief set RTC value according to calculated DecisionResult @@ -516,8 +516,7 @@ class IntersectionModule : public SceneModuleInterface * @brief act based on current RTC approval */ void reactRTCApproval( - const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason); + const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path); /** @}*/ private: diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 54af88c2f0fbb..5a66bec15bab1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -30,6 +30,14 @@ #include #include +#include +#include +#include +#include +#include +#include +#include + namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index ec174be990e66..bbe76702a5faf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -23,7 +23,11 @@ #include +#include +#include #include +#include +#include namespace autoware::behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index b694c8aaa2e3e..70e1d219c0d31 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -31,6 +31,13 @@ #include #include +#include +#include +#include +#include +#include +#include + namespace autoware::universe_utils { @@ -938,7 +945,7 @@ std::vector IntersectionModule::generateDetectionLan if (detection_lanelets.empty()) { // NOTE(soblin): due to the above filtering detection_lanelets may be empty or do not contain // conflicting_detection_lanelets - // OK to return empty detction_divsions + // OK to return empty detection_divisions return detection_divisions; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 78365bc5e500f..35f4e3b34dbee 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -26,6 +26,10 @@ #include #include +#include +#include +#include + namespace { lanelet::LineString3d getLineStringFromArcLength( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index d5cd9ca587716..3dfdcc36c0cff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -76,10 +77,9 @@ static std::optional getFirstConflictingLanelet( return std::nullopt; } -bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path) { debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::MERGE_FROM_PRIVATE_ROAD); const auto input_path = *path; @@ -152,9 +152,6 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR planning_utils::setVelocityFromIndex(stopline_idx, v, path); /* get stop point and stop factor */ - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.stop_point_pose; - planning_utils::appendStopReason(stop_factor, stop_reason); const auto & stop_pose = path->points.at(stopline_idx).point.pose; velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index a69297adea881..dc8bf1a96a7a2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -67,7 +67,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface * @brief plan go-stop velocity at traffic crossing with collision check between reference path * and object predicted path */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp index 6474435afa26e..5c9b74a9ad3bd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include namespace autoware::behavior_velocity_planner::util diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_util.cpp index d9ef145eb5ce9..959481493ccfe 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_util.cpp @@ -20,6 +20,8 @@ #include +#include + TEST(TestUtil, retrievePathsBackward) { /* @@ -89,7 +91,7 @@ TEST(TestUtil, retrievePathsBackward) } /* - TOOD(Mamoru Sobue): instantiating intersection_module and PlannerData is a messy + TODO(Mamoru Sobue): instantiating intersection_module and PlannerData is a messy class TestWithMap : public ::testing::Test { protected: diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp index 9cb37a899063d..9cb1153a8edd2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -17,6 +17,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index c3af04bbd1d1f..8365251904b18 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -37,14 +37,12 @@ NoDrivableLaneModule::NoDrivableLaneModule( velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE); } -bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path) { if (path->points.empty()) { return false; } - *stop_reason = planning_utils::initializeStopReason(StopReason::NO_DRIVABLE_LANE); - const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto & lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto & no_drivable_lane = lanelet_map_ptr->laneletLayer.get(lane_id_); @@ -82,7 +80,7 @@ bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason RCLCPP_INFO(logger_, "Approaching "); } - handle_approaching_state(path, stop_reason); + handle_approaching_state(path); break; } @@ -92,7 +90,7 @@ bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason RCLCPP_INFO(logger_, "INSIDE_NO_DRIVABLE_LANE"); } - handle_inside_no_drivable_lane_state(path, stop_reason); + handle_inside_no_drivable_lane_state(path); break; } @@ -102,7 +100,7 @@ bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason RCLCPP_INFO(logger_, "STOPPED"); } - handle_stopped_state(path, stop_reason); + handle_stopped_state(path); break; } @@ -131,7 +129,7 @@ void NoDrivableLaneModule::handle_init_state() } } -void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopReason * stop_reason) +void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) { const double longitudinal_offset = -1.0 * (planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m); @@ -163,11 +161,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR // Get stop point and stop factor { - tier4_planning_msgs::msg::StopFactor stop_factor; const auto & stop_pose = op_stop_pose.value(); - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points.push_back(stop_point); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); @@ -205,8 +199,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR } } -void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( - PathWithLaneId * path, StopReason * stop_reason) +void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * path) { const auto & current_point = planner_data_->current_odometry->pose.position; const size_t current_seg_idx = findEgoSegmentIndex(path->points); @@ -216,11 +209,7 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( // Get stop point and stop factor { - tier4_planning_msgs::msg::StopFactor stop_factor; const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points.push_back(current_point); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); @@ -239,7 +228,7 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( } } -void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReason * stop_reason) +void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path) { const auto & stopped_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, planner_data_->current_odometry->pose.position, 0.0); @@ -258,11 +247,7 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReaso // Get stop point and stop factor { - tier4_planning_msgs::msg::StopFactor stop_factor; const auto & stop_pose = ego_pos_on_path.pose; - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points.push_back(stop_pose.position); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index d9e6121a8ae51..d8c5e3e0425d1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -57,7 +57,7 @@ class NoDrivableLaneModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; @@ -79,9 +79,9 @@ class NoDrivableLaneModule : public SceneModuleInterface double distance_ego_first_intersection{}; void handle_init_state(); - void handle_approaching_state(PathWithLaneId * path, StopReason * stop_reason); - void handle_inside_no_drivable_lane_state(PathWithLaneId * path, StopReason * stop_reason); - void handle_stopped_state(PathWithLaneId * path, StopReason * stop_reason); + void handle_approaching_state(PathWithLaneId * path); + void handle_inside_no_drivable_lane_state(PathWithLaneId * path); + void handle_stopped_state(PathWithLaneId * path); void initialize_debug_data( const lanelet::Lanelet & no_drivable_lane, const geometry_msgs::msg::Point & ego_pos); }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp index 25315fd397aa4..7da73a313da79 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -23,6 +23,7 @@ #include #include +#include namespace autoware::behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index bac91d6ca92fd..3769aed71a1ec 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -52,7 +52,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( state_machine_.setMarginTime(planner_param_.state_clear_time); } -bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path) { // Store original path const auto original_path = *path; @@ -64,7 +64,6 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason // Reset data debug_data_ = no_stopping_area::DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - *stop_reason = planning_utils::initializeStopReason(StopReason::NO_STOPPING_AREA); const no_stopping_area::EgoData ego_data(*planner_data_); @@ -142,27 +141,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason // Create StopReason { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_point->second; - stop_factor.stop_factor_points = debug_data_.stuck_points; - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_point->second, VelocityFactor::UNKNOWN); } - // Create legacy StopReason - { - const double stop_path_point_distance = autoware::motion_utils::calcSignedArcLength( - path->points, 0, stop_pose.position, stop_point->first); - - if ( - !first_stop_path_point_distance_ || - stop_path_point_distance < first_stop_path_point_distance_.value()) { - debug_data_.first_stop_pose = stop_point->second; - first_stop_path_point_distance_ = stop_path_point_distance; - } - } } else if (state_machine_.getState() == StateMachine::State::GO) { // reset pass judge if current state is go pass_judge_.is_stoppable = true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 886c8f04dc702..51f3a0d261ebd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -59,7 +59,7 @@ class NoStoppingAreaModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp index 526a6baf0dd30..5ce56d756e919 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp @@ -26,6 +26,7 @@ #include #include +#include namespace autoware::behavior_velocity_planner::no_stopping_area { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index 783970c701262..b3e9b2678189f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -15,6 +15,7 @@ #include "grid_utils.hpp" #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 56a5acd8c7dc4..baf15cbebce81 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 24b9d10e09830..522e83390b0f1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -30,6 +30,7 @@ #include #include +#include #include // turn on only when debugging. @@ -82,8 +83,7 @@ OcclusionSpotModule::OcclusionSpotModule( } } -bool OcclusionSpotModule::modifyPathVelocity( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +bool OcclusionSpotModule::modifyPathVelocity(PathWithLaneId * path) { if (param_.is_show_processing_time) stop_watch_.tic("total_processing_time"); debug_data_.resetData(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index b83051fb6b6ec..35c409a9c459b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -53,7 +53,7 @@ class OcclusionSpotModule : public SceneModuleInterface /** * @brief plan occlusion spot velocity at unknown area in occupancy grid */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index 6bc9423b6dc96..73fba3969442e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -21,7 +21,9 @@ #include #include +#include #include +#include struct indexHash { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 0322bda0a88c9..8fee8ee783fa2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -22,6 +22,9 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/vector3.hpp" +#include +#include + using Point = geometry_msgs::msg::Point; using Vector3 = geometry_msgs::msg::Vector3; using DynamicObjects = autoware_perception_msgs::msg::PredictedObjects; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index b31506918a2db..49749cd1299bd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -4,8 +4,3 @@ backward_path_length: 5.0 behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 - max_accel: -2.8 - max_jerk: -5.0 - system_delay: 0.5 - delay_response_time: 0.5 - is_publish_debug_path: false # publish all debug path with lane id in each module diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 3c4f4c3fdd64c..399762f0b8607 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -11,6 +11,7 @@ + @@ -26,7 +27,6 @@ - @@ -53,6 +53,7 @@ + @@ -68,7 +69,5 @@ - - diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json index 67c9c06104b40..80044d5c6af03 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json @@ -21,47 +21,17 @@ "default": "1.0", "description": "the output path will be interpolated by this interval" }, - "max_accel": { - "type": "number", - "default": "-2.8", - "description": "(to be a global parameter) max acceleration of the vehicle" - }, - "system_delay": { - "type": "number", - "default": "0.5", - "description": "(to be a global parameter) delay time until output control command" - }, - "delay_response_time": { - "type": "number", - "default": "0.5", - "description": "(to be a global parameter) delay time of the vehicle's response to control commands" - }, "stop_line_extend_length": { "type": "number", "default": "5.0", "description": "extend length of stop line" - }, - "max_jerk": { - "type": "number", - "default": "-5.0", - "description": "max jerk of the vehicle" - }, - "is_publish_debug_path": { - "type": "boolean", - "default": "false", - "description": "is publish debug path?" } }, "required": [ "forward_path_length", "behavior_output_path_interval", "backward_path_length", - "max_accel", - "system_delay", - "delay_response_time", - "stop_line_extend_length", - "max_jerk", - "is_publish_debug_path" + "stop_line_extend_length" ], "additionalProperties": false } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 5f4d0606abfc6..d78bc883e6b35 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -28,6 +28,8 @@ #include #include #include + +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -79,8 +81,6 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Publishers path_pub_ = this->create_publisher("~/output/path", 1); - stop_reason_diag_pub_ = - this->create_publisher("~/output/stop_reason", 1); debug_viz_pub_ = this->create_publisher("~/debug/path", 1); // Parameters @@ -327,7 +327,6 @@ void BehaviorVelocityPlannerNode::onTrigger( path_pub_->publish(output_path_msg); published_time_publisher_->publish_if_subscribed(path_pub_, output_path_msg.header.stamp); - stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); if (debug_viz_pub_->get_subscription_count() > 0) { publishDebugMarker(output_path_msg); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp index d72b90d41e8f4..e260f582aae60 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp @@ -113,7 +113,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // publisher rclcpp::Publisher::SharedPtr path_pub_; - rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; void publishDebugMarker(const autoware_planning_msgs::msg::Path & path); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp index 05aa4868249f3..4820c340058ff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -25,34 +25,6 @@ namespace autoware::behavior_velocity_planner { -namespace -{ -std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) -{ - const std::string json_dumps_pose = - (boost::format( - R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % - pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % - pose.orientation.y % pose.orientation.z) - .str(); - return json_dumps_pose; -} - -diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string & stop_reason, const geometry_msgs::msg::Pose & stop_pose) -{ - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; - diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; - stop_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stop_reason_diag.name = "stop_reason"; - stop_reason_diag.message = stop_reason; - stop_reason_diag_kv.key = "stop_pose"; - stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); - stop_reason_diag.values.push_back(stop_reason_diag_kv); - return stop_reason_diag; -} -} // namespace - BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() : plugin_loader_( "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") @@ -107,34 +79,12 @@ tier4_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPat { tier4_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; - double first_stop_path_point_distance = - autoware::motion_utils::calcArcLength(output_path_msg.points); - std::string stop_reason_msg("path_end"); - for (const auto & plugin : scene_manager_plugins_) { plugin->updateSceneModuleInstances(planner_data, input_path_msg); plugin->plan(&output_path_msg); - const std::optional firstStopPathPointDistance = - plugin->getFirstStopPathPointDistance(); - - if (firstStopPathPointDistance.has_value()) { - if (firstStopPathPointDistance.value() < first_stop_path_point_distance) { - first_stop_path_point_distance = firstStopPathPointDistance.value(); - stop_reason_msg = plugin->getModuleName(); - } - } } - const geometry_msgs::msg::Pose stop_pose = autoware::motion_utils::calcInterpolatedPose( - output_path_msg.points, first_stop_path_point_distance); - - stop_reason_diag_ = makeStopReasonDiag(stop_reason_msg, stop_pose); - return output_path_msg; } -diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopReasonDiag() const -{ - return stop_reason_diag_; -} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp index abeab16dd71cb..fddd658cef06e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -46,14 +46,12 @@ class BehaviorVelocityPlannerManager void launchScenePlugin(rclcpp::Node & node, const std::string & name); void removeScenePlugin(rclcpp::Node & node, const std::string & name); + // cppcheck-suppress functionConst tier4_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, const tier4_planning_msgs::msg::PathWithLaneId & input_path_msg); - diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; - private: - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag_; pluginlib::ClassLoader plugin_loader_; std::vector> scene_manager_plugins_; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index cc2f4bf3b96b4..fe79450d0def8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -21,6 +21,8 @@ #include #include +#include +#include #include using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; @@ -49,6 +51,8 @@ std::shared_ptr generateNode() const auto autoware_test_utils_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto behavior_velocity_planner_common_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common"); const auto behavior_velocity_planner_dir = ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); const auto velocity_smoother_dir = @@ -82,25 +86,27 @@ std::shared_ptr generateNode() node_options.parameter_overrides(params); autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", - velocity_smoother_dir + "/config/Analytical.param.yaml", - behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config("blind_spot"), - get_behavior_velocity_module_config("crosswalk"), - get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config("detection_area"), - get_behavior_velocity_module_config("intersection"), - get_behavior_velocity_module_config("no_stopping_area"), - get_behavior_velocity_module_config("occlusion_spot"), - get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config("speed_bump"), - get_behavior_velocity_module_config("stop_line"), - get_behavior_velocity_module_config("traffic_light"), - get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("no_drivable_lane")}); + node_options, + {autoware_test_utils_dir + "/config/test_common.param.yaml", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", + get_behavior_velocity_module_config("blind_spot"), + get_behavior_velocity_module_config("crosswalk"), + get_behavior_velocity_module_config("walkway"), + get_behavior_velocity_module_config("detection_area"), + get_behavior_velocity_module_config("intersection"), + get_behavior_velocity_module_config("no_stopping_area"), + get_behavior_velocity_module_config("occlusion_spot"), + get_behavior_velocity_module_config("run_out"), + get_behavior_velocity_module_config("speed_bump"), + get_behavior_velocity_module_config("stop_line"), + get_behavior_velocity_module_config("traffic_light"), + get_behavior_velocity_module_config("virtual_traffic_light"), + get_behavior_velocity_module_config("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt index 9cb992312f52a..4c06c427a026d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/scene_module_interface.cpp + src/planner_data.cpp src/utilization/path_utilization.cpp src/utilization/trajectory_utils.cpp src/utilization/arc_lane_util.cpp @@ -15,15 +16,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/src/test_state_machine.cpp - test/src/test_arc_lane_util.cpp - test/src/test_utilization.cpp - ) + file(GLOB TEST_SOURCES test/src/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${TEST_SOURCES}) target_link_libraries(test_${PROJECT_NAME} gtest_main ${PROJECT_NAME} ) endif() -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE + config +) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml new file mode 100644 index 0000000000000..aff2aec9cfa29 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + max_accel: -2.8 + max_jerk: -5.0 + system_delay: 0.5 + delay_response_time: 0.5 + is_publish_debug_path: false # publish all debug path with lane id in each module diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 4075f356c187a..309ba33a3498a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -15,11 +15,10 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" #include "autoware/route_handler/route_handler.hpp" - -#include -#include -#include +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include #include @@ -38,32 +37,19 @@ #include #include -#include #include #include #include #include -#include namespace autoware::behavior_velocity_planner { -class BehaviorVelocityPlannerNode; struct PlannerData { - explicit PlannerData(rclcpp::Node & node) - : clock_(node.get_clock()), - vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) - { - max_stop_acceleration_threshold = node.declare_parameter( - "max_accel"); // TODO(someone): read min_acc in velocity_controller.param.yaml? - max_stop_jerk_threshold = node.declare_parameter("max_jerk"); - system_delay = node.declare_parameter("system_delay"); - delay_response_time = node.declare_parameter("delay_response_time"); - } + explicit PlannerData(rclcpp::Node & node); rclcpp::Clock::SharedPtr clock_; - // 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; @@ -71,85 +57,33 @@ struct PlannerData std::deque velocity_buffer; autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; - // occupancy grid + nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; - // 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; - // This variable is used when the Autoware's behavior has to depend on whether it's simulation or - // not. bool is_simulation = false; - // velocity smoother std::shared_ptr velocity_smoother_; - // route handler std::shared_ptr route_handler_; - // parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; - // additional parameters double max_stop_acceleration_threshold; double max_stop_jerk_threshold; double system_delay; double delay_response_time; double stop_line_extend_length; - bool isVehicleStopped(const double stop_duration = 0.0) const - { - if (velocity_buffer.empty()) { - return false; - } - - // Get velocities within stop_duration - const auto now = clock_->now(); - std::vector vs; - for (const auto & velocity : velocity_buffer) { - vs.push_back(velocity.twist.linear.x); - - const auto & s = velocity.header.stamp; - const auto time_diff = - now >= s ? now - s : rclcpp::Duration(0, 0); // Note: negative time throws an exception. - if (time_diff.seconds() >= stop_duration) { - break; - } - } - - // Check all velocities - constexpr double stop_velocity = 1e-3; - for (const auto & v : vs) { - if (v >= stop_velocity) { - return false; - } - } - - return true; - } - - /** - *@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 - */ + bool isVehicleStopped(const double stop_duration = 0.0) const; + std::optional getTrafficSignal( - 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)); - } + const lanelet::Id id, const bool keep_last_observation = false) const; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp index 5f6549f1e4492..4867b14cbb806 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp @@ -34,7 +34,6 @@ class PluginInterface virtual void updateSceneModuleInstances( const std::shared_ptr & planner_data, const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual std::optional getFirstStopPathPointDistance() = 0; virtual const char * getModuleName() = 0; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp index 0392bf24d3a36..44a00072a0be4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -38,10 +38,6 @@ class PluginWrapper : public PluginInterface { scene_manager_->updateSceneModuleInstances(planner_data, path); } - std::optional getFirstStopPathPointDistance() override - { - return scene_manager_->getFirstStopPathPointDistance(); - } const char * getModuleName() override { return scene_manager_->getModuleName(); } private: diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index e39cfb3a5144d..fadda66f12562 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -30,8 +30,6 @@ #include #include #include -#include -#include #include #include #include @@ -41,6 +39,7 @@ #include #include #include +#include #include // Debug @@ -61,8 +60,6 @@ using autoware::universe_utils::getOrDeclareParameter; using builtin_interfaces::msg::Time; 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; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; @@ -73,9 +70,9 @@ struct ObjectOfInterest autoware_perception_msgs::msg::Shape shape; ColorName color; ObjectOfInterest( - const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, autoware_perception_msgs::msg::Shape shape, const ColorName & color_name) - : pose(pose), shape(shape), color(color_name) + : pose(pose), shape(std::move(shape)), color(color_name) { } }; @@ -87,12 +84,13 @@ class SceneModuleInterface const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); virtual ~SceneModuleInterface() = default; - virtual bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) = 0; + virtual bool modifyPathVelocity(PathWithLaneId * path) = 0; virtual visualization_msgs::msg::MarkerArray createDebugMarkerArray() = 0; virtual std::vector createVirtualWalls() = 0; int64_t getModuleId() const { return module_id_; } + void setPlannerData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; @@ -116,8 +114,6 @@ class SceneModuleInterface std::shared_ptr getTimeKeeper() { return time_keeper_; } - std::optional getFirstStopPathPointDistance() { return first_stop_path_point_distance_; } - void setActivation(const bool activated) { activated_ = activated; } void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } bool isActivated() const { return activated_; } @@ -139,7 +135,6 @@ class SceneModuleInterface rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; std::optional infrastructure_command_; - std::optional first_stop_path_point_distance_; autoware::motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; @@ -174,8 +169,6 @@ class SceneModuleManagerInterface virtual const char * getModuleName() = 0; - std::optional getFirstStopPathPointDistance() { return first_stop_path_point_distance_; } - void updateSceneModuleInstances( const std::shared_ptr & planner_data, const tier4_planning_msgs::msg::PathWithLaneId & path); @@ -208,7 +201,6 @@ class SceneModuleManagerInterface std::shared_ptr planner_data_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_; - std::optional first_stop_path_point_distance_; rclcpp::Node & node_; rclcpp::Clock::SharedPtr clock_; // Debug @@ -217,7 +209,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::Publisher::SharedPtr pub_debug_path_; - rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_velocity_factor_; rclcpp::Publisher::SharedPtr diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 56154879ea938..fae13db2822e7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -20,7 +20,6 @@ #include -#include #include #include @@ -29,9 +28,8 @@ namespace autoware::behavior_velocity_planner { -namespace -{ -geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p) + +inline geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -40,8 +38,6 @@ geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Poi return geom_p; } -} // namespace - namespace arc_lane_utils { using PathIndexWithPose = std::pair; // front index, pose diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp index 3a1c8fdeb7a0d..5ac91cdaf1369 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp @@ -26,9 +26,7 @@ #include #include -namespace autoware::behavior_velocity_planner -{ -namespace debug +namespace autoware::behavior_velocity_planner::debug { visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, @@ -46,6 +44,6 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( const std::vector & points, const std::string & ns, const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); -} // namespace debug -} // namespace autoware::behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner::debug + #endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp index 1e45e5655f12f..2d4dab018fb18 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -20,13 +20,11 @@ #include #include -#include - namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, - tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); + 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( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp index 4d88f2bfecce8..dfe70d376621e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp @@ -19,7 +19,6 @@ #include #include -#include namespace autoware::behavior_velocity_planner { @@ -37,11 +36,11 @@ class StateMachine { if (state == State::STOP) { return "STOP"; - } else if (state == State::GO) { + } + if (state == State::GO) { return "GO"; - } else { - return ""; } + return ""; } StateMachine() { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp index 5f1c17ea1b815..fc4f852ef82f7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp @@ -15,13 +15,12 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#include +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include #include #include #include -#include #include #include @@ -37,23 +36,29 @@ namespace autoware::behavior_velocity_planner { +/** + * @brief Represents detection range parameters. + */ struct DetectionRange { - bool use_right = true; - bool use_left = true; - double interval; - double min_longitudinal_distance; - double max_longitudinal_distance; - double max_lateral_distance; - double wheel_tread; - double right_overhang; - double left_overhang; + bool use_right = true; ///< Whether to use the right side. + bool use_left = true; ///< Whether to use the left side. + double interval{0.0}; ///< Interval of detection points. + double min_longitudinal_distance{0.0}; ///< Minimum longitudinal detection distance. + double max_longitudinal_distance{0.0}; ///< Maximum longitudinal detection distance. + double max_lateral_distance{0.0}; ///< Maximum lateral detection distance. + double wheel_tread{0.0}; ///< Wheel tread of the vehicle. + double right_overhang{0.0}; ///< Right overhang of the vehicle. + double left_overhang{0.0}; ///< Left overhang of the vehicle. }; +/** + * @brief Represents a traffic signal with a timestamp. + */ struct TrafficSignalStamped { - builtin_interfaces::msg::Time stamp; - autoware_perception_msgs::msg::TrafficLightGroup signal; + builtin_interfaces::msg::Time stamp; ///< Timestamp of the signal. + autoware_perception_msgs::msg::TrafficLightGroup signal; ///< Traffic light group. }; using Pose = geometry_msgs::msg::Pose; @@ -65,29 +70,33 @@ using Polygons2d = std::vector; 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 geometry_msgs::msg::Point & point, const size_t idx); -// create detection area from given range return false if creation failure + bool createDetectionAreaPolygons( Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, const double min_velocity = 1.0); + Point2d calculateOffsetPoint2d( const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y); + void extractClosePartition( const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, BasicPolygons2d & close_partition, const double distance_thresh = 30.0); -void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys); + +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr & ll, BasicPolygons2d & polys); + void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input); + void insertVelocity( PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, size_t & insert_index, const double min_distance = 0.001); + inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); @@ -99,6 +108,7 @@ geometry_msgs::msg::Pose getAheadPose( 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( const double velocity, const double max_stop_acceleration, const double delay_response_time); @@ -114,10 +124,6 @@ double findReachTime( const double jerk, const double accel, const double velocity, const double distance, const double t_min, const double t_max); -StopReason initializeStopReason(const std::string & stop_reason); - -void appendStopReason(const StopFactor stop_factor, StopReason * stop_reason); - std::vector toRosPoints(const PredictedObjects & object); LineString2d extendLine( @@ -217,6 +223,7 @@ bool isOverLine( std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); + std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output); @@ -225,11 +232,11 @@ std::optional insertStopPoint( or lane-changeable parent lanes with `lane` and has same turn_direction value. */ std::set getAssociativeIntersectionLanelets( - lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map, + const lanelet::ConstLanelet & lane, const lanelet::LaneletMapPtr lanelet_map, const lanelet::routing::RoutingGraphPtr routing_graph); lanelet::ConstLanelets getConstLaneletsFromIds( - lanelet::LaneletMapConstPtr map, const std::set & ids); + const lanelet::LaneletMapConstPtr & map, const std::set & ids); } // namespace planning_utils } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 6692358f1b21f..105b0528940bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -52,6 +52,7 @@ ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json new file mode 100644 index 0000000000000..2468b71aa9be1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json @@ -0,0 +1,59 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Behavior Velocity Planner Common", + "type": "object", + "definitions": { + "behavior_velocity_planner_common": { + "type": "object", + "properties": { + "max_accel": { + "type": "number", + "default": "-2.8", + "description": "(to be a global parameter) max acceleration of the vehicle" + }, + "system_delay": { + "type": "number", + "default": "0.5", + "description": "(to be a global parameter) delay time until output control command" + }, + "delay_response_time": { + "type": "number", + "default": "0.5", + "description": "(to be a global parameter) delay time of the vehicle's response to control commands" + }, + "max_jerk": { + "type": "number", + "default": "-5.0", + "description": "max jerk of the vehicle" + }, + "is_publish_debug_path": { + "type": "boolean", + "default": "false", + "description": "is publish debug path?" + } + }, + "required": [ + "max_accel", + "system_delay", + "delay_response_time", + "max_jerk", + "is_publish_debug_path" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/behavior_velocity_planner_common" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp new file mode 100644 index 0000000000000..df2a183e3bfb3 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp @@ -0,0 +1,72 @@ +// Copyright 2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_velocity_planner_common/planner_data.hpp" + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include + +namespace autoware::behavior_velocity_planner +{ +PlannerData::PlannerData(rclcpp::Node & node) +: clock_(node.get_clock()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) +{ + max_stop_acceleration_threshold = node.declare_parameter("max_accel"); + max_stop_jerk_threshold = node.declare_parameter("max_jerk"); + system_delay = node.declare_parameter("system_delay"); + delay_response_time = node.declare_parameter("delay_response_time"); +} + +bool PlannerData::isVehicleStopped(const double stop_duration) const +{ + if (velocity_buffer.empty()) { + return false; + } + + const auto now = clock_->now(); + std::vector vs; + for (const auto & velocity : velocity_buffer) { + vs.push_back(velocity.twist.linear.x); + + const auto & s = velocity.header.stamp; + const auto time_diff = + now >= s ? now - s : rclcpp::Duration(0, 0); // Note: negative time throws an exception. + if (time_diff.seconds() >= stop_duration) { + break; + } + } + + constexpr double stop_velocity = 1e-3; + for (const auto & v : vs) { + if (v >= stop_velocity) { + return false; + } + } + + return true; +} + +std::optional PlannerData::getTrafficSignal( + const lanelet::Id id, const bool keep_last_observation) 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::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index 2d014d4bbf81d..cdfde1ce51205 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -22,6 +22,8 @@ #include #include #include +#include +#include namespace autoware::behavior_velocity_planner { @@ -67,8 +69,6 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( std::string("~/virtual_wall/") + module_name, 5); pub_velocity_factor_ = node.create_publisher( std::string("/planning/velocity_factors/") + module_name, 1); - pub_stop_reason_ = - node.create_publisher("~/output/stop_reasons", 1); pub_infrastructure_commands_ = node.create_publisher( "~/output/infrastructure_commands", 1); @@ -107,40 +107,28 @@ void SceneModuleManagerInterface::modifyPathVelocity( StopWatch stop_watch; stop_watch.tic("Total"); visualization_msgs::msg::MarkerArray debug_marker_array; - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - stop_reason_array.header.frame_id = "map"; - stop_reason_array.header.stamp = clock_->now(); velocity_factor_array.header.frame_id = "map"; velocity_factor_array.header.stamp = clock_->now(); tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; infrastructure_command_array.stamp = clock_->now(); - first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points); for (const auto & scene_module : scene_modules_) { - tier4_planning_msgs::msg::StopReason stop_reason; scene_module->resetVelocityFactor(); scene_module->setPlannerData(planner_data_); - scene_module->modifyPathVelocity(path, &stop_reason); + scene_module->modifyPathVelocity(path); // The velocity factor must be called after modifyPathVelocity. const auto velocity_factor = scene_module->getVelocityFactor(); if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { velocity_factor_array.factors.emplace_back(velocity_factor); } - if (stop_reason.reason != "") { - stop_reason_array.stop_reasons.emplace_back(stop_reason); - } if (const auto command = scene_module->getInfrastructureCommand()) { infrastructure_command_array.commands.push_back(*command); } - if (scene_module->getFirstStopPathPointDistance() < first_stop_path_point_distance_) { - first_stop_path_point_distance_ = scene_module->getFirstStopPathPointDistance(); - } - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { debug_marker_array.markers.push_back(marker); } @@ -148,9 +136,6 @@ void SceneModuleManagerInterface::modifyPathVelocity( virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); } - if (!stop_reason_array.stop_reasons.empty()) { - pub_stop_reason_->publish(stop_reason_array); - } pub_velocity_factor_->publish(velocity_factor_array); pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index da6ef7262de74..9b02b374b0a6c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -26,10 +26,7 @@ #include #endif -#include -#include #include -#include namespace { @@ -54,12 +51,6 @@ geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & p, const d return multiplied_p; } -/* -geometry_msgs::msg::Point operator*(const double v, const geometry_msgs::msg::Point & p) -{ -return p * v; -} -*/ } // namespace namespace autoware::behavior_velocity_planner::arc_lane_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp index 33cf428e80808..d9162419ec33f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp @@ -16,8 +16,6 @@ #include -#include -#include namespace autoware::behavior_velocity_planner { @@ -27,8 +25,8 @@ Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & rig polygon.outer().push_back(left_line.front()); - for (auto itr = right_line.begin(); itr != right_line.end(); ++itr) { - polygon.outer().push_back(*itr); + for (const auto & itr : right_line) { + polygon.outer().push_back(itr); } for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) { @@ -57,8 +55,8 @@ geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) geometry_msgs::msg::Polygon polygon_msg; geometry_msgs::msg::Point32 point_msg; for (const auto & p : polygon.outer()) { - point_msg.x = p.x(); - point_msg.y = p.y(); + point_msg.x = static_cast(p.x()); + point_msg.y = static_cast(p.y()); polygon_msg.points.push_back(point_msg); } return polygon_msg; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index dfd201bac707c..2ddf62bb584ff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -15,14 +15,13 @@ #include #include #include -namespace autoware::behavior_velocity_planner -{ -namespace debug + +#include +#include +namespace autoware::behavior_velocity_planner::debug { -using autoware::universe_utils::appendMarkerArray; using autoware::universe_utils::createDefaultMarker; using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray createPolygonMarkerArray( @@ -33,8 +32,9 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( visualization_msgs::msg::MarkerArray msg; { auto marker = createDefaultMarker( - "map", now, ns.c_str(), module_id, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(x, y, z), createMarkerColor(r, g, b, 0.8)); + "map", now, ns, static_cast(module_id), visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(static_cast(x), static_cast(y), static_cast(z)), + createMarkerColor(static_cast(r), static_cast(g), static_cast(b), 0.8f)); marker.lifetime = rclcpp::Duration::from_seconds(0.3); for (const auto & p : polygon.points) { @@ -64,15 +64,18 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( const auto & p = path.points.at(i); auto marker = createDefaultMarker( - "map", now, ns.c_str(), planning_utils::bitShift(lane_id) + i, - visualization_msgs::msg::Marker::ARROW, createMarkerScale(x, y, z), - createMarkerColor(r, g, b, 0.999)); + "map", now, ns, static_cast(planning_utils::bitShift(lane_id) + i), + visualization_msgs::msg::Marker::ARROW, + createMarkerScale(static_cast(x), static_cast(y), static_cast(z)), + createMarkerColor( + static_cast(r), static_cast(g), static_cast(b), 0.999f)); marker.lifetime = rclcpp::Duration::from_seconds(0.3); marker.pose = p.point.pose; if (std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end()) { // if p.lane_ids has lane_id - marker.color = createMarkerColor(r, g, b, 0.999); + marker.color = createMarkerColor( + static_cast(r), static_cast(g), static_cast(b), 0.999f); } else { marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.999); } @@ -90,13 +93,13 @@ visualization_msgs::msg::MarkerArray createObjectsMarkerArray( auto marker = createDefaultMarker( "map", now, ns, 0, visualization_msgs::msg::Marker::CUBE, createMarkerScale(3.0, 1.0, 1.0), - createMarkerColor(r, g, b, 0.8)); + createMarkerColor(static_cast(r), static_cast(g), static_cast(b), 0.8f)); marker.lifetime = rclcpp::Duration::from_seconds(1.0); for (size_t i = 0; i < objects.objects.size(); ++i) { const auto & object = objects.objects.at(i); - marker.id = planning_utils::bitShift(module_id) + i; + marker.id = static_cast(planning_utils::bitShift(module_id) + i); marker.pose = object.kinematics.initial_pose_with_covariance.pose; msg.markers.push_back(marker); } @@ -113,15 +116,14 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( auto marker = createDefaultMarker( "map", now, ns, 0, visualization_msgs::msg::Marker::SPHERE, createMarkerScale(x, y, z), - createMarkerColor(r, g, b, 0.999)); + createMarkerColor(static_cast(r), static_cast(g), static_cast(b), 0.999f)); marker.lifetime = rclcpp::Duration::from_seconds(0.3); for (size_t i = 0; i < points.size(); ++i) { - marker.id = i + planning_utils::bitShift(module_id); + marker.id = static_cast(i + planning_utils::bitShift(module_id)); marker.pose.position = points.at(i); msg.markers.push_back(marker); } return msg; } -} // namespace debug -} // namespace autoware::behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner::debug diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp index 17ea92022ea13..b8e6e28bae7c2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -18,14 +18,13 @@ #include #include -#include #include namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, - tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger) + 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."); @@ -156,12 +155,12 @@ autoware_planning_msgs::msg::Path filterStopPathPoint( { 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) { + for (auto & point : filtered_path.points) { + if (std::fabs(point.longitudinal_velocity_mps) < 0.01) { found_stop = true; } if (found_stop) { - filtered_path.points.at(i).longitudinal_velocity_mps = 0.0; + point.longitudinal_velocity_mps = 0.0; } } return filtered_path; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 6815ca3ff8cb4..cb9b58ec48924 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -25,6 +25,10 @@ #include +#include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -75,13 +79,15 @@ bool smoothPath( TrajectoryPoints clipped; TrajectoryPoints traj_smoothed; clipped.insert( - clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + clipped.end(), traj_resampled.begin() + static_cast(traj_resampled_closest), + traj_resampled.end()); if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; return false; } traj_smoothed.insert( - traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); + traj_smoothed.begin(), traj_resampled.begin(), + traj_resampled.begin() + static_cast(traj_resampled_closest)); if (external_v_limit) { autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index ccb2660dbf3c6..a8f909201ac7f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" + +#include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware_lanelet2_extension/utility/query.hpp" #include @@ -32,7 +33,9 @@ #endif #include -#include +#include +#include +#include #include #include @@ -64,7 +67,7 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con PathPoint p; p.pose = autoware::universe_utils::calcInterpolatedPose(p0, p1, ratio); const double v = lerp(p0.longitudinal_velocity_mps, p1.longitudinal_velocity_mps, ratio); - p.longitudinal_velocity_mps = v; + p.longitudinal_velocity_mps = static_cast(v); return p; } @@ -91,19 +94,11 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( } // namespace -namespace autoware::behavior_velocity_planner -{ -namespace planning_utils +namespace autoware::behavior_velocity_planner::planning_utils { -using autoware::motion_utils::calcLongitudinalOffsetToSegment; using autoware::motion_utils::calcSignedArcLength; -using autoware::motion_utils::validateNonEmpty; -using autoware::universe_utils::calcAzimuthAngle; using autoware::universe_utils::calcDistance2d; using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::calcSquaredDistance2d; -using autoware::universe_utils::createQuaternionFromYaw; -using autoware::universe_utils::getPoint; using autoware_planning_msgs::msg::PathPoint; size_t calcSegmentIndexFromPointIndex( @@ -154,7 +149,7 @@ bool createDetectionAreaPolygons( const double offset_right = (da_range.wheel_tread / 2.0) + da_range.right_overhang; //! max index is the last index of path point - const size_t max_index = static_cast(path.points.size() - 1); + const auto max_index = static_cast(path.points.size() - 1); //! avoid bug with same point polygon const double eps = 1e-3; auto nearest_idx = @@ -246,10 +241,9 @@ void extractClosePartition( close_partition.emplace_back(p); } } - return; } -void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys) +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr & ll, BasicPolygons2d & polys) { const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll); for (const auto & partition : partitions) { @@ -259,7 +253,7 @@ void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons } // correct line to calculate distance in accurate boost::geometry::correct(line); - polys.emplace_back(lanelet::BasicPolygon2d(line)); + polys.emplace_back(line); } } @@ -269,7 +263,6 @@ void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLane input->points.at(i).point.longitudinal_velocity_mps = std::min(static_cast(vel), input->points.at(i).point.longitudinal_velocity_mps); } - return; } void insertVelocity( @@ -309,7 +302,7 @@ geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, const tier4_planning_msgs::msg::PathWithLaneId & path) { - if (path.points.size() == 0) { + if (path.points.empty()) { return geometry_msgs::msg::Pose{}; } @@ -327,7 +320,8 @@ geometry_msgs::msg::Pose getAheadPose( p.position.x = w_p0 * p0.position.x + w_p1 * p1.position.x; p.position.y = w_p0 * p0.position.y + w_p1 * p1.position.y; p.position.z = w_p0 * p0.position.z + w_p1 * p1.position.z; - tf2::Quaternion q0_tf, q1_tf; + tf2::Quaternion q0_tf; + tf2::Quaternion q1_tf; tf2::fromMsg(p0.orientation, q0_tf); tf2::fromMsg(p1.orientation, q1_tf); p.orientation = tf2::toMsg(q0_tf.slerp(q1_tf, w_p1)); @@ -424,15 +418,16 @@ double findReachTime( const int warn_iter = 100; double lower = min; double upper = max; - double t; + double t = NAN; int iter = 0; - for (int i = 0;; i++) { + while (true) { t = 0.5 * (lower + upper); const double fx = f(t, j, a, v, d); // std::cout<<"fx: "< 0.0) { + } + if (fx > 0.0) { upper = t; } else { lower = t; @@ -475,31 +470,18 @@ double calcDecelerationVelocityFromDistanceToTarget( const double t_jerk = findReachTime(j_max, a0, v0, l, 0, t_const_jerk); const double velocity = vt(t_jerk, j_max, a0, v0); return velocity; - } else { - const double v1 = vt(t_const_jerk, j_max, a0, v0); - const double discriminant_of_stop = 2.0 * a_max * d_const_acc_stop + v1 * v1; - // case3: distance to target is farther than distance to stop - if (discriminant_of_stop <= 0) { - return 0.0; - } - // case2: distance to target is within constant accel deceleration - // solve d = 0.5*a^2+v*t by t - const double t_acc = (-v1 + std::sqrt(discriminant_of_stop)) / a_max; - return vt(t_acc, 0.0, a_max, v1); } - return current_velocity; -} -StopReason initializeStopReason(const std::string & stop_reason) -{ - StopReason stop_reason_msg; - stop_reason_msg.reason = stop_reason; - return stop_reason_msg; -} - -void appendStopReason(const StopFactor stop_factor, StopReason * stop_reason) -{ - stop_reason->stop_factors.emplace_back(stop_factor); + const double v1 = vt(t_const_jerk, j_max, a0, v0); + const double discriminant_of_stop = 2.0 * a_max * d_const_acc_stop + v1 * v1; + // case3: distance to target is farther than distance to stop + if (discriminant_of_stop <= 0) { + return 0.0; + } + // case2: distance to target is within constant accel deceleration + // solve d = 0.5*a^2+v*t by t + const double t_acc = (-v1 + std::sqrt(discriminant_of_stop)) / a_max; + return vt(t_acc, 0.0, a_max, v1); } std::vector toRosPoints(const PredictedObjects & object) @@ -558,6 +540,7 @@ std::vector getLaneletsOnPath( } std::vector lanelets; + lanelets.reserve(unique_lane_ids.size()); for (const auto lane_id : unique_lane_ids) { lanelets.push_back(lanelet_map->laneletLayer.get(lane_id)); } @@ -599,7 +582,7 @@ std::vector getSubsequentLaneIdsSetOnPath( // cannot find base_index in all_lane_ids if (base_index == all_lane_ids.end()) { - return std::vector(); + return {}; } std::vector subsequent_lane_ids; @@ -673,11 +656,11 @@ std::optional insertStopPoint( } std::set getAssociativeIntersectionLanelets( - lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map, + const lanelet::ConstLanelet & lane, const lanelet::LaneletMapPtr lanelet_map, const lanelet::routing::RoutingGraphPtr routing_graph) { const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - if (turn_direction.compare("else") == 0) { + if (turn_direction == "else") { return {}; } @@ -702,7 +685,7 @@ std::set getAssociativeIntersectionLanelets( } lanelet::ConstLanelets getConstLaneletsFromIds( - lanelet::LaneletMapConstPtr map, const std::set & ids) + const lanelet::LaneletMapConstPtr & map, const std::set & ids) { lanelet::ConstLanelets ret{}; for (const auto & id : ids) { @@ -712,5 +695,4 @@ lanelet::ConstLanelets getConstLaneletsFromIds( return ret; } -} // namespace planning_utils -} // namespace autoware::behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner::planning_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp similarity index 70% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp index ca5c57fd522c9..51c6b5b9dda04 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp @@ -22,6 +22,7 @@ #include #include +#include #define DEBUG_PRINT_PATH(path) \ { \ @@ -175,3 +176,70 @@ TEST(specialInterpolation, specialInterpolation) EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); } } + +TEST(filterLitterPathPoint, nominal) +{ + using autoware::behavior_velocity_planner::filterLitterPathPoint; + using autoware_planning_msgs::msg::Path; + using autoware_planning_msgs::msg::PathPoint; + + const auto genPath = [](const std::vector & px, const std::vector & vx) { + Path path; + for (size_t i = 0; i < px.size(); ++i) { + PathPoint point; + point.pose.position.x = px[i]; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = static_cast(vx[i]); + path.points.push_back(point); + } + return path; + }; + + const std::vector px{0.0, 1.0, 1.001, 2.0, 3.0}; + const std::vector vx{5.0, 3.5, 3.5, 3.0, 2.5}; + + const auto path = genPath(px, vx); + const auto filtered_path = filterLitterPathPoint(path); + + ASSERT_EQ(filtered_path.points.size(), 4U); // Expected: Points at x = {0.0, 1.0, 2.0, 3.0} + EXPECT_DOUBLE_EQ(filtered_path.points[0].pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(filtered_path.points[1].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(filtered_path.points[2].pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(filtered_path.points[3].pose.position.x, 3.0); + + EXPECT_DOUBLE_EQ(filtered_path.points[0].longitudinal_velocity_mps, 5.0); + EXPECT_DOUBLE_EQ(filtered_path.points[1].longitudinal_velocity_mps, 3.5); + EXPECT_DOUBLE_EQ(filtered_path.points[2].longitudinal_velocity_mps, 3.0); + EXPECT_DOUBLE_EQ(filtered_path.points[3].longitudinal_velocity_mps, 2.5); +} + +TEST(filterStopPathPoint, nominal) +{ + using autoware::behavior_velocity_planner::filterStopPathPoint; + using autoware_planning_msgs::msg::Path; + using autoware_planning_msgs::msg::PathPoint; + + const auto genPath = [](const std::vector & px, const std::vector & vx) { + Path path; + for (size_t i = 0; i < px.size(); ++i) { + PathPoint point; + point.pose.position.x = px[i]; + point.longitudinal_velocity_mps = static_cast(vx[i]); + path.points.push_back(point); + } + return path; + }; + + const std::vector px{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector vx{5.0, 4.0, 0.0, 2.0, 3.0}; + + const auto path = genPath(px, vx); + const auto filtered_path = filterStopPathPoint(path); + + ASSERT_EQ(filtered_path.points.size(), 5U); + EXPECT_DOUBLE_EQ(filtered_path.points[0].longitudinal_velocity_mps, 5.0); + EXPECT_DOUBLE_EQ(filtered_path.points[1].longitudinal_velocity_mps, 4.0); + EXPECT_DOUBLE_EQ(filtered_path.points[2].longitudinal_velocity_mps, 0.0); + EXPECT_DOUBLE_EQ(filtered_path.points[3].longitudinal_velocity_mps, 0.0); + EXPECT_DOUBLE_EQ(filtered_path.points[4].longitudinal_velocity_mps, 0.0); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp new file mode 100644 index 0000000000000..5ed490ab755a9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp @@ -0,0 +1,113 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp" +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" + +#include +#include + +#include +#include + +#include + +#include + +TEST(smoothPath, nominal) +{ + using autoware::behavior_velocity_planner::smoothPath; + using tier4_planning_msgs::msg::PathPointWithLaneId; + using tier4_planning_msgs::msg::PathWithLaneId; + + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + options.arguments( + {"--ros-args", "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common") + + "/config/behavior_velocity_planner_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_vehicle_info.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother") + + "/config/default_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother") + + "/config/default_velocity_smoother.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother") + + "/config/JerkFiltered.param.yaml"}); + auto node = std::make_shared("test_node", options); + + auto planner_data = std::make_shared(*node); + planner_data->stop_line_extend_length = 5.0; + planner_data->vehicle_info_.max_longitudinal_offset_m = 1.0; + + planner_data->current_odometry = std::make_shared([]() { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 2.0; + pose.pose.position.y = 1.0; + return pose; + }()); + + planner_data->current_velocity = std::make_shared([]() { + geometry_msgs::msg::TwistStamped twist; + twist.twist.linear.x = 3.0; + return twist; + }()); + + planner_data->current_acceleration = + std::make_shared([]() { + geometry_msgs::msg::AccelWithCovarianceStamped accel; + accel.accel.accel.linear.x = 1.0; + return accel; + }()); + + planner_data->velocity_smoother_ = + std::make_shared( + *node, std::make_shared()); + + // Input path + PathWithLaneId in_path; + for (double i = 0; i <= 10.0; i += 1.0) { + PathPointWithLaneId point; + point.point.pose.position.x = i; + point.point.pose.position.y = 0.0; + point.point.longitudinal_velocity_mps = 5.0; // Set constant velocity + in_path.points.emplace_back(point); + } + + // Output path + PathWithLaneId out_path; + + // Execute smoothing + auto result = smoothPath(in_path, out_path, planner_data); + + // Check results + EXPECT_TRUE(result); + + // Check initial and last points + EXPECT_DOUBLE_EQ(out_path.points.front().point.pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(out_path.points.front().point.pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(out_path.points.back().point.pose.position.x, 10.0); + EXPECT_DOUBLE_EQ(out_path.points.back().point.pose.position.y, 0.0); + + for (auto & point : out_path.points) { + // Check velocities + EXPECT_LE( + point.point.longitudinal_velocity_mps, + 5.0); // Smoothed velocity must not exceed initial + } +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp new file mode 100644 index 0000000000000..8c8ef575b5a9a --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp @@ -0,0 +1,308 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "./utils.hpp" +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" + +#include +#include +#include +#include +#include + +#include + +using namespace autoware::behavior_velocity_planner; // NOLINT +using namespace autoware::behavior_velocity_planner::planning_utils; // NOLINT +using autoware_planning_msgs::msg::PathPoint; + +TEST(PlanningUtilsTest, calcSegmentIndexFromPointIndex) +{ + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point point; + point.x = 4.5; + point.y = 0.0; + + size_t result = calcSegmentIndexFromPointIndex(path.points, point, 4); + + EXPECT_EQ(result, 4); +} + +TEST(PlanningUtilsTest, calculateOffsetPoint2d) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + pose.orientation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1)); // No rotation + + double offset_x = 1.0; + double offset_y = 1.0; + + auto result = calculateOffsetPoint2d(pose, offset_x, offset_y); + + EXPECT_NEAR(result.x(), 1.0, 0.1); + EXPECT_NEAR(result.y(), 1.0, 0.1); +} + +TEST(PlanningUtilsTest, createDetectionAreaPolygons) +{ + // using namespace autoware::behavior_velocity_planner::planning_utils; + + // Input parameters + Polygons2d da_polys; + tier4_planning_msgs::msg::PathWithLaneId path; + geometry_msgs::msg::Pose target_pose; + size_t target_seg_idx = 0; + autoware::behavior_velocity_planner::DetectionRange da_range; + + da_range.min_longitudinal_distance = 1.0; + da_range.max_longitudinal_distance = 10.0; + da_range.max_lateral_distance = 2.0; + da_range.interval = 5.0; + da_range.wheel_tread = 1.0; + da_range.left_overhang = 0.5; + da_range.right_overhang = 0.5; + da_range.use_left = true; + da_range.use_right = true; + + double obstacle_vel_mps = 0.5; + double min_velocity = 1.0; + + // Path with some points + for (double i = 0.0; i < 3.0; ++i) { + tier4_planning_msgs::msg::PathPointWithLaneId point; + point.point.pose.position.x = i * 5.0; + point.point.pose.position.y = 0.0; + point.point.longitudinal_velocity_mps = 1.0; + path.points.push_back(point); + } + + // Target pose + target_pose.position.x = 0.0; + target_pose.position.y = 0.0; + + // Call the function + bool success = createDetectionAreaPolygons( + da_polys, path, target_pose, target_seg_idx, da_range, obstacle_vel_mps, min_velocity); + + // Assert success + EXPECT_TRUE(success); + + // Validate results + ASSERT_FALSE(da_polys.empty()); + EXPECT_EQ(da_polys.size(), 2); // Expect polygons for left and right bounds + + // Check the first polygon + auto & polygon = da_polys.front(); + EXPECT_EQ(polygon.outer().size(), 7); // Each polygon should be a rectangle + + // Check some specific points + EXPECT_NEAR(polygon.outer()[0].x(), 1.0, 0.1); // Left inner bound + EXPECT_NEAR(polygon.outer()[0].y(), 1.0, 0.1); +} + +// Test for calcJudgeLineDistWithAccLimit +TEST(PlanningUtilsTest, calcJudgeLineDistWithAccLimit) +{ + double velocity = 10.0; // m/s + double max_stop_acceleration = -3.0; // m/s^2 + double delay_response_time = 1.0; // s + + double result = + calcJudgeLineDistWithAccLimit(velocity, max_stop_acceleration, delay_response_time); + + EXPECT_NEAR(result, 26.67, 0.01); // Updated expected value +} + +// Test for calcJudgeLineDistWithJerkLimit +TEST(PlanningUtilsTest, calcJudgeLineDistWithJerkLimit) +{ + double velocity = 10.0; // m/s + double acceleration = 0.0; // m/s^2 + double max_stop_acceleration = -3.0; // m/s^2 + double max_stop_jerk = -1.0; // m/s^3 + double delay_response_time = 1.0; // s + + double result = calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); + + EXPECT_GT(result, 0.0); // The result should be positive +} + +// Test for isAheadOf +TEST(PlanningUtilsTest, isAheadOf) +{ + geometry_msgs::msg::Pose target; + geometry_msgs::msg::Pose origin; + target.position.x = 10.0; + target.position.y = 0.0; + origin.position.x = 0.0; + origin.position.y = 0.0; + origin.orientation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1)); // No rotation + + EXPECT_TRUE(isAheadOf(target, origin)); + + target.position.x = -10.0; + EXPECT_FALSE(isAheadOf(target, origin)); +} + +TEST(PlanningUtilsTest, insertDecelPoint) +{ + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point stop_point; + stop_point.x = 5.0; + stop_point.y = 0.0; + + auto stop_pose = insertDecelPoint(stop_point, path, 5.0); + ASSERT_TRUE(stop_pose.has_value()); + EXPECT_NEAR(stop_pose->position.x, 5.0, 0.1); +} + +// Test for insertVelocity +TEST(PlanningUtilsTest, insertVelocity) +{ + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + tier4_planning_msgs::msg::PathPointWithLaneId path_point; + path_point.point.pose.position.x = 5.0; + path_point.point.pose.position.y = 0.0; + path_point.point.longitudinal_velocity_mps = 10.0; + + size_t insert_index = 5; + insertVelocity(path, path_point, 10.0, insert_index); + + EXPECT_EQ(path.points.size(), 11); + EXPECT_NEAR(path.points.at(insert_index).point.longitudinal_velocity_mps, 10.0, 0.1); +} + +// Test for insertStopPoint +TEST(PlanningUtilsTest, insertStopPoint) +{ + { + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point stop_point; + stop_point.x = 5.0; + stop_point.y = 0.0; + + auto stop_pose = insertStopPoint(stop_point, path); + ASSERT_TRUE(stop_pose.has_value()); + EXPECT_NEAR(stop_pose->position.x, 5.0, 0.1); + } + { + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point stop_point; + stop_point.x = 5.0; + stop_point.y = 0.0; + + auto stop_pose = insertStopPoint(stop_point, 4, path); + ASSERT_TRUE(stop_pose.has_value()); + EXPECT_NEAR(stop_pose->position.x, 5.0, 0.1); + } +} + +// Test for getAheadPose +TEST(PlanningUtilsTest, getAheadPose) +{ + tier4_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathPointWithLaneId point1; + tier4_planning_msgs::msg::PathPointWithLaneId point2; + tier4_planning_msgs::msg::PathPointWithLaneId point3; + point1.point.pose.position.x = 0.0; + point2.point.pose.position.x = 5.0; + point3.point.pose.position.x = 10.0; + + path.points.emplace_back(point1); + path.points.emplace_back(point2); + path.points.emplace_back(point3); + + double ahead_dist = 7.0; + auto pose = getAheadPose(0, ahead_dist, path); + + EXPECT_NEAR(pose.position.x, 7.0, 0.1); +} + +TEST(PlanningUtilsTest, calcDecelerationVelocityFromDistanceToTarget) +{ + double max_slowdown_jerk = -1.0; // m/s^3 + double max_slowdown_accel = -3.0; // m/s^2 + double current_accel = -1.0; // m/s^2 + double current_velocity = 10.0; // m/s + double distance_to_target = 100.0; // m + + double result = calcDecelerationVelocityFromDistanceToTarget( + max_slowdown_jerk, max_slowdown_accel, current_accel, current_velocity, distance_to_target); + + EXPECT_LT(result, current_velocity); +} + +// Test for toRosPoints +TEST(PlanningUtilsTest, ToRosPoints) +{ + using autoware_perception_msgs::msg::PredictedObject; + PredictedObjects objects; + + // Add a predicted object + PredictedObject obj1; + obj1.kinematics.initial_pose_with_covariance.pose.position.x = 1.0; + obj1.kinematics.initial_pose_with_covariance.pose.position.y = 2.0; + obj1.kinematics.initial_pose_with_covariance.pose.position.z = 3.0; + objects.objects.push_back(obj1); + + // Add another predicted object + PredictedObject obj2; + obj2.kinematics.initial_pose_with_covariance.pose.position.x = 4.0; + obj2.kinematics.initial_pose_with_covariance.pose.position.y = 5.0; + obj2.kinematics.initial_pose_with_covariance.pose.position.z = 6.0; + objects.objects.push_back(obj2); + + auto points = toRosPoints(objects); + + ASSERT_EQ(points.size(), 2); // Verify the number of points + EXPECT_EQ(points[0].x, 1.0); + EXPECT_EQ(points[0].y, 2.0); + EXPECT_EQ(points[0].z, 3.0); + EXPECT_EQ(points[1].x, 4.0); + EXPECT_EQ(points[1].y, 5.0); + EXPECT_EQ(points[1].z, 6.0); +} + +// Test for extendLine +TEST(PlanningUtilsTest, ExtendLine) +{ + lanelet::ConstPoint3d point1(lanelet::InvalId, 0.0, 0.0, 0.0); + lanelet::ConstPoint3d point2(lanelet::InvalId, 1.0, 1.0, 0.0); + double length = 1.0; + + auto extended_line = extendLine(point1, point2, length); + + ASSERT_EQ(extended_line.size(), 2); // Verify the line has two points + + // Check the extended line coordinates + EXPECT_NEAR(extended_line[0].x(), -0.707, 0.001); // Extended in the reverse direction + EXPECT_NEAR(extended_line[0].y(), -0.707, 0.001); + EXPECT_NEAR(extended_line[1].x(), 1.707, 0.001); // Extended in the forward direction + EXPECT_NEAR(extended_line[1].y(), 1.707, 0.001); +} + +TEST(PlanningUtilsTest, getConstLaneletsFromIds) +{ + const auto package_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); + lanelet::LaneletMapPtr map = + autoware::test_utils::loadMap(package_dir + "/test_map/lanelet2_map.osm"); + + auto lanelets = getConstLaneletsFromIds(map, {10333, 10310, 10291}); + + EXPECT_EQ(lanelets.size(), 3); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp index 983a371234ccf..2823648e184d3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -20,6 +20,9 @@ #include #include +#include +#include + using autoware::universe_utils::appendMarkerArray; using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::createDefaultMarker; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 74b1ca490f752..a15f232b75a37 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -28,7 +28,9 @@ #include #include +#include #include +#include namespace autoware::behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index f294e698ba8c2..4a16f263d0a54 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -16,6 +16,7 @@ #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp index cf3c0fd7e10e1..e4e08d69a79f7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp @@ -15,6 +15,9 @@ #include "path_utils.hpp" #include + +#include +#include namespace autoware::behavior_velocity_planner::run_out_utils { /** diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index c3f91d0506388..a32a65a0d8909 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -32,7 +32,9 @@ #include #include +#include #include +#include namespace autoware::behavior_velocity_planner { @@ -63,8 +65,7 @@ void RunOutModule::setPlannerParam(const PlannerParam & planner_param) planner_param_ = planner_param; } -bool RunOutModule::modifyPathVelocity( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +bool RunOutModule::modifyPathVelocity(PathWithLaneId * path) { // timer starts const auto t_start = std::chrono::system_clock::now(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 4f326aef298be..3673a215bb749 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -49,7 +49,7 @@ class RunOutModule : public SceneModuleInterface std::unique_ptr dynamic_obstacle_creator, const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.cpp index 0a23d2218feb0..dafb4e6da097b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.cpp @@ -14,6 +14,8 @@ #include "state_machine.hpp" +#include + namespace autoware::behavior_velocity_planner { namespace run_out_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp index c32d38da2db33..9a9a49c3cb081 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp @@ -25,6 +25,8 @@ #include #include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp index dfe5d4de85300..54ea807f3268b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp @@ -20,6 +20,9 @@ #include +#include +#include + namespace autoware::behavior_velocity_planner { using autoware::motion_utils::calcSignedArcLength; @@ -70,8 +73,7 @@ SpeedBumpModule::SpeedBumpModule( } } -bool SpeedBumpModule::modifyPathVelocity( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +bool SpeedBumpModule::modifyPathVelocity(PathWithLaneId * path) { if (path->points.empty()) { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index b909dc80689d5..176a01d5112c5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -56,7 +56,7 @@ class SpeedBumpModule : public SceneModuleInterface const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt index 402eb5e20aa24..f4528f0d13cf4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt @@ -11,4 +11,15 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_scene.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml index 7b1d82a46f652..f5e00fa7087ae 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml @@ -3,8 +3,4 @@ stop_line: stop_margin: 0.0 stop_duration_sec: 1.0 - use_initialization_stop_line_state: true hold_stop_margin_distance: 2.0 - - debug: - show_stop_line_collision_check: false # [-] whether to show stopline collision diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml index e24f167eb2058..c5e3690dd620d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml @@ -33,6 +33,7 @@ tf2_geometry_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp index 573a260138679..fa94bfbaa361b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp @@ -12,89 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" #include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" #include "scene.hpp" -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; - -namespace -{ -using DebugData = StopLineModule::DebugData; - -visualization_msgs::msg::MarkerArray createStopLineCollisionCheck( - const DebugData & debug_data, const int64_t module_id) -{ - visualization_msgs::msg::MarkerArray msg; - - // Search Segments - { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.ns = "search_segments"; - marker.id = module_id; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - marker.action = visualization_msgs::msg::Marker::ADD; - for (const auto & e : debug_data.search_segments) { - marker.points.push_back( - geometry_msgs::build().x(e.at(0).x()).y(e.at(0).y()).z(0.0)); - marker.points.push_back( - geometry_msgs::build().x(e.at(1).x()).y(e.at(1).y()).z(0.0)); - } - marker.scale = createMarkerScale(0.1, 0.1, 0.1); - marker.color = createMarkerColor(0.0, 0.0, 1.0, 0.999); - msg.markers.push_back(marker); - } - - // Search stopline - { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.ns = "search_stopline"; - marker.id = module_id; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - const auto p0 = debug_data.search_stopline.at(0); - marker.points.push_back( - geometry_msgs::build().x(p0.x()).y(p0.y()).z(0.0)); - const auto p1 = debug_data.search_stopline.at(1); - marker.points.push_back( - geometry_msgs::build().x(p1.x()).y(p1.y()).z(0.0)); - - marker.scale = createMarkerScale(0.1, 0.1, 0.1); - marker.color = createMarkerColor(1.0, 0.0, 0.0, 0.999); - msg.markers.push_back(marker); - } - - return msg; -} - -} // namespace - -visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray() -{ - visualization_msgs::msg::MarkerArray debug_marker_array; - if (planner_param_.show_stop_line_collision_check) { - appendMarkerArray( - createStopLineCollisionCheck(debug_data_, module_id_), &debug_marker_array, - this->clock_->now()); - } - return debug_marker_array; -} autoware::motion_utils::VirtualWalls StopLineModule::createVirtualWalls() { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index ffdcea16b45b5..b305a1d2aa404 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -15,7 +15,8 @@ #include "manager.hpp" #include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware_lanelet2_extension/utility/query.hpp" + +#include #include #include @@ -28,7 +29,7 @@ using autoware::universe_utils::getOrDeclareParameter; using lanelet::TrafficSign; StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterface(node, getModuleName()) +: SceneModuleManagerInterface(node, getModuleName()), planner_param_() { const std::string ns(StopLineModuleManager::getModuleName()); auto & p = planner_param_; @@ -36,11 +37,6 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) p.hold_stop_margin_distance = getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); p.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); - p.use_initialization_stop_line_state = - getOrDeclareParameter(node, ns + ".use_initialization_stop_line_state"); - // debug - p.show_stop_line_collision_check = - getOrDeclareParameter(node, ns + ".debug.show_stop_line_collision_check"); } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( @@ -82,10 +78,9 @@ void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pat for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { const auto module_id = stop_line_with_lane_id.first.id(); - const auto lane_id = stop_line_with_lane_id.second; if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, lane_id, stop_line_with_lane_id.first, planner_param_, + module_id, stop_line_with_lane_id.first, planner_param_, logger_.get_child("stop_line_module"), clock_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp index b83a4f94e9a1f..bef8a5eef4ac0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp @@ -15,7 +15,6 @@ #ifndef MANAGER_HPP_ #define MANAGER_HPP_ -#include "autoware/behavior_velocity_planner_common/plugin_interface.hpp" #include "autoware/behavior_velocity_planner_common/plugin_wrapper.hpp" #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" #include "scene.hpp" diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 6a4b85cd6926c..4d8dea94583bb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -15,28 +15,39 @@ #include "scene.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" -#include "autoware/trajectory/path_point_with_lane_id.hpp" -#include +#include + +#include + +#include +#include namespace autoware::behavior_velocity_planner { +geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line) +{ + geometry_msgs::msg::Point center_point; + center_point.x = (stop_line[0].x() + stop_line[1].x()) / 2.0; + center_point.y = (stop_line[0].y() + stop_line[1].y()) / 2.0; + center_point.z = (stop_line[0].z() + stop_line[1].z()) / 2.0; + return center_point; +} + StopLineModule::StopLineModule( - const int64_t module_id, const size_t lane_id, lanelet::ConstLineString3d stop_line, - const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) + const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), - lane_id_(lane_id), stop_line_(std::move(stop_line)), - state_(State::APPROACH), planner_param_(planner_param), + state_(State::APPROACH), debug_data_() { velocity_factor_.init(PlanningBehavior::STOP_SIGN); } -bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) { auto trajectory = trajectory::Trajectory::Builder{}.build( @@ -46,132 +57,133 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop return true; } - debug_data_ = DebugData(); - const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - debug_data_.base_link2front = base_link2front; - first_stop_path_point_distance_ = trajectory->length(); - *stop_reason = planning_utils::initializeStopReason(StopReason::STOP_LINE); - - const LineString2d stop_line = planning_utils::extendLine( - stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); - - // Calculate intersection with stop line - const auto trajectory_stop_line_intersection = - trajectory->crossed(stop_line.front(), stop_line.back()); + auto [ego_s, stop_point] = + getEgoAndStopPoint(*trajectory, planner_data_->current_odometry->pose, state_); - // If no collision found, do nothing - if (!trajectory_stop_line_intersection) { - RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision"); + if (!stop_point) { return true; } - const double stop_point_s = - *trajectory_stop_line_intersection - - (base_link2front + planner_param_.stop_margin); // consider vehicle length and stop margin + trajectory->longitudinal_velocity_mps.range(*stop_point, trajectory->length()).set(0.0); - if (stop_point_s < 0.0) { - return true; - } + path->points = trajectory->restore(); - const auto stop_pose = trajectory->compute(stop_point_s); + updateVelocityFactor(&velocity_factor_, state_, *stop_point - ego_s); - /** - * @brief : calculate signed arc length consider stop margin from stop line - * - * |----------------------------| - * s---ego----------x--|--------g - */ - const double ego_on_trajectory_s = - trajectory->closest(planner_data_->current_odometry->pose.position); - const double signed_arc_dist_to_stop_point = stop_point_s - ego_on_trajectory_s; + updateStateAndStoppedTime( + &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); - switch (state_) { - case State::APPROACH: { - // Insert stop pose - trajectory->longitudinal_velocity_mps.range(stop_point_s, trajectory->length()).set(0.0); - - // Update first stop path point distance - first_stop_path_point_distance_ = stop_point_s; - debug_data_.stop_pose = stop_pose.point.pose; - - // Get stop point and stop factor - { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_pose.point.pose; - stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); - planning_utils::appendStopReason(stop_factor, stop_reason); - velocity_factor_.set(signed_arc_dist_to_stop_point, VelocityFactor::APPROACHING); - } + geometry_msgs::msg::Pose stop_pose = trajectory->compute(*stop_point).point.pose; - // Move to stopped state if stopped - if ( - signed_arc_dist_to_stop_point < planner_param_.hold_stop_margin_distance && - planner_data_->isVehicleStopped()) { - RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); + updateDebugData(&debug_data_, stop_pose, state_); - state_ = State::STOPPED; - stopped_time_ = std::make_shared(clock_->now()); + return true; +} - if (signed_arc_dist_to_stop_point < -planner_param_.hold_stop_margin_distance) { - RCLCPP_ERROR( - logger_, "Failed to stop near stop line but ego stopped. Change state to STOPPED"); - } +std::pair> StopLineModule::getEgoAndStopPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & ego_pose, + const State & state) const +{ + const double ego_s = trajectory.closest(ego_pose.position); + std::optional stop_point_s; + + switch (state) { + case State::APPROACH: { + const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const LineString2d stop_line = planning_utils::extendLine( + stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); + + // Calculate intersection with stop line + const auto trajectory_stop_line_intersection = + trajectory.crossed(stop_line.front(), stop_line.back()); + + // If no collision found, do nothing + if (!trajectory_stop_line_intersection) { + stop_point_s = std::nullopt; + break; } + stop_point_s = + *trajectory_stop_line_intersection - + (base_link2front + planner_param_.stop_margin); // consider vehicle length and stop margin + + if (*stop_point_s < 0.0) { + stop_point_s = std::nullopt; + } break; } case State::STOPPED: { - // Insert stop pose - trajectory->longitudinal_velocity_mps.range(ego_on_trajectory_s, trajectory->length()) - .set(0.0); - const auto ego_pos_on_path = trajectory->compute(ego_on_trajectory_s).point.pose; - debug_data_.stop_pose = ego_pos_on_path; - - // Get stop point and stop factor - { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = ego_pos_on_path; - stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); - planning_utils::appendStopReason(stop_factor, stop_reason); - velocity_factor_.set(signed_arc_dist_to_stop_point, VelocityFactor::STOPPED); - } + stop_point_s = ego_s; + break; + } + + case State::START: { + stop_point_s = std::nullopt; + break; + } + } + return {ego_s, stop_point_s}; +} - const double elapsed_time = (clock_->now() - *stopped_time_).seconds(); +void StopLineModule::updateStateAndStoppedTime( + State * state, std::optional * stopped_time, const rclcpp::Time & now, + const double & distance_to_stop_point, const bool & is_vehicle_stopped) const +{ + switch (*state) { + case State::APPROACH: { + if (distance_to_stop_point < planner_param_.hold_stop_margin_distance && is_vehicle_stopped) { + *state = State::STOPPED; + *stopped_time = now; + RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); - if (planner_param_.stop_duration_sec < elapsed_time) { + if (distance_to_stop_point < 0.0) { + RCLCPP_WARN(logger_, "Vehicle cannot stop before stop line"); + } + } + break; + } + case State::STOPPED: { + double stop_duration = (now - **stopped_time).seconds(); + if (stop_duration > planner_param_.stop_duration_sec) { + *state = State::START; + stopped_time->reset(); RCLCPP_INFO(logger_, "STOPPED -> START"); - state_ = State::START; } - break; } - case State::START: { - // Initialize if vehicle is far from stop_line - if (planner_param_.use_initialization_stop_line_state) { - if (signed_arc_dist_to_stop_point > planner_param_.hold_stop_margin_distance) { - RCLCPP_INFO(logger_, "START -> APPROACH"); - state_ = State::APPROACH; - } - } - break; } } +} - path->points = trajectory->restore(); - - return true; +void StopLineModule::updateVelocityFactor( + autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, + const double & distance_to_stop_point) +{ + switch (state) { + case State::APPROACH: { + velocity_factor->set(distance_to_stop_point, VelocityFactor::APPROACHING); + break; + } + case State::STOPPED: { + velocity_factor->set(distance_to_stop_point, VelocityFactor::STOPPED); + break; + } + case State::START: + break; + } } -geometry_msgs::msg::Point StopLineModule::getCenterOfStopLine( - const lanelet::ConstLineString3d & stop_line) +void StopLineModule::updateDebugData( + DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const { - geometry_msgs::msg::Point center_point; - center_point.x = (stop_line[0].x() + stop_line[1].x()) / 2.0; - center_point.y = (stop_line[0].y() + stop_line[1].y()) / 2.0; - center_point.z = (stop_line[0].z() + stop_line[1].z()) / 2.0; - return center_point; + debug_data->base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + debug_data->stop_pose = stop_pose; + if (state == State::START) { + debug_data->stop_pose = std::nullopt; + } } + } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index cb48aabe57c1a..9ac1463da8618 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -14,75 +14,106 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ +#define EIGEN_MPL2_ONLY #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" -#include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/motion_utils/factor/velocity_factor_interface.hpp" +#include "autoware/trajectory/path_point_with_lane_id.hpp" -#include -#include -#include - -#define EIGEN_MPL2_ONLY #include #include #include +#include + +#include #include +#include +#include + namespace autoware::behavior_velocity_planner { class StopLineModule : public SceneModuleInterface { - using StopLineWithLaneId = std::pair; - public: + using StopLineWithLaneId = std::pair; + using Trajectory = + autoware::trajectory::Trajectory; enum class State { APPROACH, STOPPED, START }; struct DebugData { - double base_link2front; - boost::optional stop_pose; - std::vector search_segments; - LineString2d search_stopline; + double base_link2front; ///< Distance from the base link to the vehicle front. + std::optional stop_pose; ///< Pose of the stop position. }; struct PlannerParam { - double stop_margin; - double stop_duration_sec; - double hold_stop_margin_distance; - bool use_initialization_stop_line_state; - bool show_stop_line_collision_check; + double stop_margin; ///< Margin to the stop line. + double stop_duration_sec; ///< Required stop duration at the stop line. + double + hold_stop_margin_distance; ///< Distance threshold for transitioning to the STOPPED state }; + /** + * @brief Constructor for StopLineModule. + * @param module_id Unique ID for the module. + * @param stop_line Stop line data. + * @param planner_param Planning parameters. + * @param logger Logger for output messages. + * @param clock Shared clock instance. + */ StopLineModule( - const int64_t module_id, const size_t lane_id, lanelet::ConstLineString3d stop_line, + const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; - - visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + bool modifyPathVelocity(PathWithLaneId * path) override; + + /** + * @brief Calculate ego position and stop point. + * @param trajectory Current trajectory. + * @param ego_pose Current pose of the vehicle. + * @param state Current state of the stop line module. + * @return Pair of ego position and optional stop point. + */ + std::pair> getEgoAndStopPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & ego_pose, + const State & state) const; + + /** + * @brief Update the state and stopped time of the module. + * @param state Pointer to the current state. + * @param stopped_time Pointer to the stopped time. + * @param now Current time. + * @param distance_to_stop_point Distance to the stop point. + * @param is_vehicle_stopped Flag indicating if the vehicle is stopped. + */ + void updateStateAndStoppedTime( + State * state, std::optional * stopped_time, const rclcpp::Time & now, + const double & distance_to_stop_point, const bool & is_vehicle_stopped) const; + + static void updateVelocityFactor( + autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, + const double & distance_to_stop_point); + + void updateDebugData( + DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override + { + return visualization_msgs::msg::MarkerArray{}; + } autoware::motion_utils::VirtualWalls createVirtualWalls() override; private: - std::shared_ptr stopped_time_; - - geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line); - - int64_t lane_id_; - - lanelet::ConstLineString3d stop_line_; - - // State machine - State state_; - - // Parameter - PlannerParam planner_param_; - - // Debug - DebugData debug_data_; + const lanelet::ConstLineString3d stop_line_; ///< Stop line geometry. + const PlannerParam planner_param_; ///< Parameters for the planner. + State state_; ///< Current state of the module. + std::optional stopped_time_; ///< Time when the vehicle stopped. + DebugData debug_data_; ///< Debug information. }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp new file mode 100644 index 0000000000000..1eafb71eb403c --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -0,0 +1,143 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/scene.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include +#include + +using autoware::behavior_velocity_planner::StopLineModule; + +tier4_planning_msgs::msg::PathPointWithLaneId path_point(double x, double y) +{ + tier4_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = x; + p.point.pose.position.y = y; + return p; +} + +class StopLineModuleTest : public ::testing::Test +{ +protected: + // Set up the test case + void SetUp() override + { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + options.arguments( + {"--ros-args", "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common") + + "/config/behavior_velocity_planner_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_vehicle_info.param.yaml"}); + node_ = std::make_shared("test_node", options); + + // Initialize parameters, logger, and clock + planner_param_.stop_margin = 0.5; + planner_param_.stop_duration_sec = 2.0; + planner_param_.hold_stop_margin_distance = 0.5; + + planner_data_ = std::make_shared(*node_); + planner_data_->stop_line_extend_length = 5.0; + planner_data_->vehicle_info_.max_longitudinal_offset_m = 1.0; + + stop_line_ = lanelet::ConstLineString3d( + lanelet::utils::getId(), {lanelet::Point3d(lanelet::utils::getId(), 7.0, -1.0, 0.0), + lanelet::Point3d(lanelet::utils::getId(), 7.0, 1.0, 0.0)}); + + trajectory_ = *StopLineModule::Trajectory::Builder{}.build( + {path_point(0.0, 0.0), path_point(1.0, 0.0), path_point(2.0, 0.0), path_point(3.0, 0.0), + path_point(4.0, 0.0), path_point(5.0, 0.0), path_point(6.0, 0.0), path_point(7.0, 0.0), + path_point(8.0, 0.0), path_point(9.0, 0.0), path_point(10.0, 0.0)}); + + clock_ = std::make_shared(); + + module_ = std::make_shared( + 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_); + + module_->setPlannerData(planner_data_); + } + + void TearDown() override { rclcpp::shutdown(); } + + StopLineModule::Trajectory trajectory_; + StopLineModule::PlannerParam planner_param_{}; + lanelet::ConstLineString3d stop_line_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr module_; + std::shared_ptr planner_data_; + + rclcpp::Node::SharedPtr node_; +}; + +TEST_F(StopLineModuleTest, TestGetEgoAndStopPoint) +{ + // Prepare trajectory and other parameters + geometry_msgs::msg::Pose ego_pose; + ego_pose.position.x = 5.0; + ego_pose.position.y = 1.0; + + // Execute the function + auto [ego_s, stop_point_s] = + module_->getEgoAndStopPoint(trajectory_, ego_pose, StopLineModule::State::APPROACH); + + // Verify results + EXPECT_DOUBLE_EQ(ego_s, 5.0); + EXPECT_DOUBLE_EQ(stop_point_s.value(), 7.0 - 0.5 - 1.0); + + std::tie(ego_s, stop_point_s) = + module_->getEgoAndStopPoint(trajectory_, ego_pose, StopLineModule::State::STOPPED); + + EXPECT_DOUBLE_EQ(ego_s, 5.0); + EXPECT_DOUBLE_EQ(stop_point_s.value(), 5.0); +} + +TEST_F(StopLineModuleTest, TestUpdateStateAndStoppedTime) +{ + StopLineModule::State state = StopLineModule::State::APPROACH; + std::optional stopped_time; + double distance_to_stop_point = 0.1; + bool is_vehicle_stopped = true; + + // Simulate clock progression + auto test_start_time = clock_->now(); + stopped_time = test_start_time; + + // Execute the function + module_->updateStateAndStoppedTime( + &state, &stopped_time, test_start_time, distance_to_stop_point, is_vehicle_stopped); + + // Verify state transition to STOPPED + EXPECT_EQ(state, StopLineModule::State::STOPPED); + EXPECT_TRUE(stopped_time.has_value()); + + // Simulate time elapsed to exceed stop duration + auto now = test_start_time + rclcpp::Duration::from_seconds(3.0); + module_->updateStateAndStoppedTime( + &state, &stopped_time, now, distance_to_stop_point, is_vehicle_stopped); + + // Verify state transition to START + EXPECT_EQ(state, StopLineModule::State::START); + EXPECT_FALSE(stopped_time.has_value()); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index 0394b0c9e381f..3ce8502baaf63 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -42,8 +42,7 @@ autoware::motion_utils::VirtualWalls TemplateModule::createVirtualWalls() return vw; } -bool TemplateModule::modifyPathVelocity( - [[maybe_unused]] PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +bool TemplateModule::modifyPathVelocity([[maybe_unused]] PathWithLaneId * path) { RCLCPP_INFO_ONCE(logger_, "Template Module is executing!"); return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index 9789ee37aa669..70cd5cb1235e7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -38,10 +38,9 @@ class TemplateModule : public SceneModuleInterface * specific criteria. * * @param path A pointer to the path containing points to be modified. - * @param stop_reason A pointer to the stop reason data. * @return [bool] wether the path velocity was modified or not. */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; /** * @brief Create a visualization of debug markers. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/scripts/dilemma_zone_plotter.py b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/scripts/dilemma_zone_plotter.py index 55bfbeff893b1..072979a2cef48 100755 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/scripts/dilemma_zone_plotter.py +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/scripts/dilemma_zone_plotter.py @@ -38,7 +38,7 @@ def get_params_from_yaml(): # get parameters from behavior velocity planner behavior_vel_yaml_file_path = os.path.join( autoware_launch_package_path, - "config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml", + "config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner_common.param.yaml", ) with open(behavior_vel_yaml_file_path, "r") as yaml_file: params = yaml.safe_load(yaml_file) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 0d7c77d5e1b59..b6747724ba6f7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -57,34 +57,20 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat velocity_factor_array.header.frame_id = "map"; velocity_factor_array.header.stamp = clock_->now(); - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; - stop_reason_array.header.frame_id = "map"; - stop_reason_array.header.stamp = path->header.stamp; - - first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points); nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { - tier4_planning_msgs::msg::StopReason stop_reason; std::shared_ptr traffic_light_scene_module( std::dynamic_pointer_cast(scene_module)); traffic_light_scene_module->resetVelocityFactor(); traffic_light_scene_module->setPlannerData(planner_data_); - traffic_light_scene_module->modifyPathVelocity(path, &stop_reason); + traffic_light_scene_module->modifyPathVelocity(path); // The velocity factor must be called after modifyPathVelocity. const auto velocity_factor = traffic_light_scene_module->getVelocityFactor(); if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { velocity_factor_array.factors.emplace_back(velocity_factor); } - if (stop_reason.reason != "") { - stop_reason_array.stop_reasons.emplace_back(stop_reason); - } - if ( - traffic_light_scene_module->getFirstStopPathPointDistance() < - first_stop_path_point_distance_) { - first_stop_path_point_distance_ = traffic_light_scene_module->getFirstStopPathPointDistance(); - } if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < nearest_ref_stop_path_point_index_) { @@ -102,9 +88,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat virtual_wall_marker_creator_.add_virtual_walls( traffic_light_scene_module->createVirtualWalls()); } - if (!stop_reason_array.stop_reasons.empty()) { - pub_stop_reason_->publish(stop_reason_array); - } pub_velocity_factor_->publish(velocity_factor_array); pub_debug_->publish(debug_marker_array); pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index c59071f2043bb..5eb0d5aa5f267 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -25,6 +25,8 @@ #include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -54,13 +56,11 @@ TrafficLightModule::TrafficLightModule( planner_param_ = planner_param; } -bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path) { debug_data_ = DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points); first_ref_stop_path_point_index_ = static_cast(path->points.size()) - 1; - *stop_reason = planning_utils::initializeStopReason(StopReason::TRAFFIC_LIGHT); const auto input_path = *path; @@ -133,8 +133,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Decide whether to stop or pass even if a stop signal is received. if (!isPassthrough(signed_arc_length_to_stop_point)) { - *path = - insertStopPose(input_path, stop_line.value().first, stop_line.value().second, stop_reason); + *path = insertStopPose(input_path, stop_line.value().first, stop_line.value().second); is_prev_state_stop_ = true; } return true; @@ -275,7 +274,7 @@ bool TrafficLightModule::isTrafficSignalTimedOut() const 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) + const Eigen::Vector2d & target_point) { tier4_planning_msgs::msg::PathWithLaneId modified_path; modified_path = input; @@ -292,24 +291,9 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( size_t insert_index = insert_target_point_idx; planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index); - const double target_velocity_point_distance = autoware::motion_utils::calcArcLength(std::vector( - modified_path.points.begin(), modified_path.points.begin() + target_velocity_point_idx)); - if (target_velocity_point_distance < first_stop_path_point_distance_) { - first_stop_path_point_distance_ = target_velocity_point_distance; - debug_data_.first_stop_pose = target_point_with_lane_id.point.pose; - } - - // Get stop point and stop factor - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.first_stop_pose; - if (debug_data_.highest_confidence_traffic_light_point != std::nullopt) { - stop_factor.stop_factor_points = std::vector{ - debug_data_.highest_confidence_traffic_light_point.value()}; - } velocity_factor_.set( modified_path.points, planner_data_->current_odometry->pose, target_point_with_lane_id.point.pose, VelocityFactor::UNKNOWN); - planning_utils::appendStopReason(stop_factor, stop_reason); return modified_path; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index e35d9f86fbe46..8221bb3740273 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -71,7 +71,7 @@ class TrafficLightModule : public SceneModuleInterface lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; @@ -90,7 +90,7 @@ class TrafficLightModule : public SceneModuleInterface 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); + const Eigen::Vector2d & target_point); bool isPassthrough(const double & signed_arc_length) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp index c9eb93abb2e2f..9b13123380ce1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp @@ -19,6 +19,8 @@ #include #include +#include + namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -32,8 +34,8 @@ auto getOffsetPoint(const Eigen::Vector2d & src, const Eigen::Vector2d & dst, co } auto findNearestCollisionPoint( - const LineString2d & line1, const LineString2d & line2, - const Point2d & origin) -> std::optional + const LineString2d & line1, const LineString2d & line2, const Point2d & origin) + -> std::optional { std::vector collision_points; bg::intersection(line1, line2, collision_points); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp index c6655064d3ffa..ad4ed84cea116 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp @@ -48,8 +48,8 @@ auto getOffsetPoint(const Eigen::Vector2d & src, const Eigen::Vector2d & dst, co * @return intersection point. if there is no intersection point, return std::nullopt. */ auto findNearestCollisionPoint( - const LineString2d & line1, const LineString2d & line2, - const Point2d & origin) -> std::optional; + const LineString2d & line1, const LineString2d & line2, const Point2d & origin) + -> std::optional; /** * @brief find intersection point between path and stop line and return the point. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 2be77b2731fe0..2b4acd7ff0ca9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -18,6 +18,8 @@ #include #include #include + +#include using autoware::motion_utils::createStopVirtualWallMarker; using autoware::universe_utils::appendMarkerArray; using autoware::universe_utils::createDefaultMarker; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index e60e2a42e3a47..206fb14b6d41c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -97,11 +97,10 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( logger_ = logger_.get_child((map_data_.instrument_type + "_" + map_data_.instrument_id).c_str()); } -bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path) { // Initialize setInfrastructureCommand({}); - *stop_reason = planning_utils::initializeStopReason(StopReason::VIRTUAL_TRAFFIC_LIGHT); module_data_ = {}; @@ -150,7 +149,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe // Stop at stop_line if no message received if (!virtual_traffic_light_state) { RCLCPP_DEBUG(logger_, "no message received"); - insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); + insertStopVelocityAtStopLine(path, end_line_idx); updateInfrastructureCommand(); return true; } @@ -158,7 +157,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe // Stop at stop_line if no right is given if (!hasRightOfWay(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "no right is given"); - insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); + insertStopVelocityAtStopLine(path, end_line_idx); updateInfrastructureCommand(); return true; } @@ -167,7 +166,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe if (isBeforeStopLine(end_line_idx)) { if (isStateTimeout(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "state is timeout before stop line"); - insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); + insertStopVelocityAtStopLine(path, end_line_idx); } updateInfrastructureCommand(); @@ -181,7 +180,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe if ( planner_param_.check_timeout_after_stop_line && isStateTimeout(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "state is timeout after stop line"); - insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); + insertStopVelocityAtStopLine(path, end_line_idx); updateInfrastructureCommand(); return true; } @@ -189,7 +188,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe // Stop at stop_line if finalization isn't completed if (!virtual_traffic_light_state->is_finalized) { RCLCPP_DEBUG(logger_, "finalization isn't completed"); - insertStopVelocityAtEndLine(path, stop_reason, end_line_idx); + insertStopVelocityAtEndLine(path, end_line_idx); if (isNearAnyEndLine(end_line_idx) && planner_data_->isVehicleStopped()) { state_ = State::FINALIZING; @@ -207,15 +206,6 @@ void VirtualTrafficLightModule::updateInfrastructureCommand() setInfrastructureCommand(command_); } -void VirtualTrafficLightModule::setStopReason( - const geometry_msgs::msg::Pose & stop_pose, tier4_planning_msgs::msg::StopReason * stop_reason) -{ - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points.push_back(toMsg(map_data_.instrument_center)); - planning_utils::appendStopReason(stop_factor, stop_reason); -} - std::optional VirtualTrafficLightModule::getPathIndexOfFirstEndLine() { std::optional min_seg_idx; @@ -376,8 +366,7 @@ bool VirtualTrafficLightModule::hasRightOfWay( } void VirtualTrafficLightModule::insertStopVelocityAtStopLine( - tier4_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) + tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) { const auto collision = findLastCollisionBeforeEndLine(path->points, *map_data_.stop_line, end_line_idx); @@ -428,7 +417,6 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } // Set StopReason - setStopReason(stop_pose, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN, command_.type); @@ -439,8 +427,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } void VirtualTrafficLightModule::insertStopVelocityAtEndLine( - tier4_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) + tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) { const auto collision = findLastCollisionBeforeEndLine(path->points, map_data_.end_lines, end_line_idx); @@ -463,7 +450,6 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( } // Set StopReason - setStopReason(stop_pose, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index e4e58e4288354..b031ba5b2bb2b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -79,7 +79,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; @@ -96,8 +96,6 @@ class VirtualTrafficLightModule : public SceneModuleInterface void updateInfrastructureCommand(); - void setStopReason(const Pose & stop_pose, StopReason * stop_reason); - void setVelocityFactor( const geometry_msgs::msg::Pose & stop_pose, autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor); @@ -119,12 +117,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface bool hasRightOfWay(const tier4_v2x_msgs::msg::VirtualTrafficLightState & state); void insertStopVelocityAtStopLine( - tier4_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); + tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); void insertStopVelocityAtEndLine( - tier4_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); + tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); }; } // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp index 940e29f0a0c0e..a1950bf768f4c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp @@ -20,6 +20,8 @@ #include #include +#include +#include namespace autoware::behavior_velocity_planner::virtual_traffic_light { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp index dd01b1be7fb83..e6bfa0234951c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp @@ -19,7 +19,9 @@ #include +#include #include +#include using autoware::behavior_velocity_planner::virtual_traffic_light::calcCenter; using autoware::behavior_velocity_planner::virtual_traffic_light::calcHeadPose; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 765732969951d..6a7505930a334 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -18,6 +18,7 @@ #include #include +#include namespace autoware::behavior_velocity_planner { @@ -81,12 +82,11 @@ std::pair WalkwayModule::getStopLine( return std::make_pair(dist_ego_to_stop, p_stop_line); } -bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path) { const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_ = DebugData(planner_data_); - *stop_reason = planning_utils::initializeStopReason(StopReason::WALKWAY); const auto input = *path; @@ -119,10 +119,6 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ } /* get stop point and stop factor */ - StopFactor stop_factor; - stop_factor.stop_pose = stop_pose.value(); - stop_factor.stop_factor_points.push_back(path_end_points_on_walkway->first); - planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose.value(), VelocityFactor::UNKNOWN); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index a400f57451d2e..df54eafd11cc2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -46,7 +46,7 @@ class WalkwayModule : public SceneModuleInterface const PlannerParam & planner_param, const bool use_regulatory_element, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp index 3f544c792d8e6..21726c43e12d6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp @@ -17,6 +17,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp index 020b15ba5133b..79b817c51169c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp @@ -17,6 +17,7 @@ #include #include +#include using autoware::motion_velocity_planner::obstacle_velocity_limiter::CollisionChecker; using autoware::motion_velocity_planner::obstacle_velocity_limiter::linestring_t; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/debug.cpp index 4d9859dba90da..ea83aafebfbb7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/debug.cpp @@ -20,6 +20,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/map_utils.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/map_utils.cpp index 1796c65b42de0..c0894673d81f3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/map_utils.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/map_utils.cpp @@ -23,6 +23,8 @@ #include #include +#include +#include namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index 1f788a73a3f42..e501b756af6a1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -23,6 +23,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 35135089c0c8c..3fa3ec7cbf782 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -33,6 +33,9 @@ #include #include +#include +#include +#include namespace autoware::motion_velocity_planner { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp index 0b8aad36580e6..d7b8e128b3cee 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp @@ -24,6 +24,7 @@ #include #include +#include constexpr auto EPS = 1e-15; constexpr auto EPS_APPROX = 1e-3; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp index 6e84b63a3c7fc..f16c23efcb14c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp @@ -14,6 +14,10 @@ #include "autoware/motion_velocity_planner_common/collision_checker.hpp" +#include +#include +#include + namespace autoware::motion_velocity_planner { CollisionChecker::CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp index 5988272b39ae1..d2a4a4560a430 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include using autoware::motion_velocity_planner::CollisionChecker; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 0e8a36977fec8..96865fec1c197 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include namespace diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index 416ad215d5e25..b08798cbef772 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -18,6 +18,7 @@ #include #include +#include namespace autoware::motion_velocity_planner { 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 index c35749ef33710..44ff7820c1566 100644 --- 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 @@ -20,6 +20,8 @@ #include +#include +#include #include using autoware::motion_velocity_planner::MotionVelocityPlannerNode; diff --git a/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp index 9a882ad06659a..e7a1901fc83ab 100644 --- a/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp @@ -15,6 +15,9 @@ #include #include +#include +#include +#include namespace autoware::bezier_sampler { diff --git a/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp index e9a1a9ef32de5..c36b8f88c8486 100644 --- a/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp @@ -14,6 +14,8 @@ #include +#include + namespace autoware::bezier_sampler { std::vector sample( diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index faabd0bcfdbe5..cab4acfbb8556 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -30,6 +30,7 @@ #include #include #include +#include namespace autoware::frenet_planner { diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp index 34905f79cd364..fd07516d61bbf 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -27,8 +27,11 @@ #include +#include #include #include +#include +#include namespace autoware::path_sampler { diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp index 127393abc5ac3..7f313318af236 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include diff --git a/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp index ce473b1768a54..6c9447f25def0 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp @@ -18,6 +18,7 @@ #include #include #include +#include namespace autoware::sampler_common::transform { diff --git a/planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp b/planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp index 6c5f43e929766..8229f5b9c9e41 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp @@ -17,8 +17,10 @@ #include #include +#include #include #include +#include constexpr auto TOL = 1E-6; // 1µm tolerance @@ -103,9 +105,9 @@ TEST(splineTransform, benchFrenet) EXPECT_NEAR(frenet_naive.d, frenet_lut.d, precision); } std::cout << "size = " << size << std::endl; - std::cout << "\tnaive: " << std::chrono::duration_cast(naive).count() - << "ms\n"; - std::cout << "\tlut : " << std::chrono::duration_cast(lut).count() + std::cout << "\t naive: " + << std::chrono::duration_cast(naive).count() << "ms\n"; + std::cout << "\t lut : " << std::chrono::duration_cast(lut).count() << "ms\n"; } } diff --git a/sensing/autoware_cuda_utils/CHANGELOG.rst b/sensing/autoware_cuda_utils/CHANGELOG.rst index f5437230cc80d..fe3b28768acbb 100644 --- a/sensing/autoware_cuda_utils/CHANGELOG.rst +++ b/sensing/autoware_cuda_utils/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cuda_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_cuda_utils +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.38.0 (2024-11-08) ------------------- diff --git a/sensing/autoware_gnss_poser/README.md b/sensing/autoware_gnss_poser/README.md index 9619038492af0..cdd67bde027d2 100644 --- a/sensing/autoware_gnss_poser/README.md +++ b/sensing/autoware_gnss_poser/README.md @@ -17,7 +17,7 @@ If the transformation from `base_link` to the antenna cannot be obtained, it out | Name | Type | Description | | ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ | -| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectorInfo` | map projection info | +| `/map/map_projector_info` | `autoware_map_msgs::msg::MapProjectorInfo` | map projection info | | `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | | `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) | diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp index 829e0e3ee8d81..681e9ae70a89c 100644 --- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp @@ -16,6 +16,7 @@ #include +#include #include namespace autoware::image_diagnostics diff --git a/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp index 18d7965470f05..2fbd5d4a6a39e 100644 --- a/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp @@ -17,6 +17,8 @@ #include #include +#include + namespace autoware::imu_corrector { diff --git a/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp index ec91311455911..8dcc024ffc2b4 100644 --- a/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp @@ -19,7 +19,9 @@ #include #include +#include #include +#include namespace autoware::imu_corrector { diff --git a/sensing/autoware_imu_corrector/src/imu_corrector_core.cpp b/sensing/autoware_imu_corrector/src/imu_corrector_core.cpp index a9acb45888945..d615a721ca5eb 100644 --- a/sensing/autoware_imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/autoware_imu_corrector/src/imu_corrector_core.cpp @@ -14,6 +14,9 @@ #include "imu_corrector_core.hpp" +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/sensing/autoware_imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/autoware_imu_corrector/test/test_gyro_bias_estimation_module.cpp index 573dbe84d6027..2d75c6d50de55 100644 --- a/sensing/autoware_imu_corrector/test/test_gyro_bias_estimation_module.cpp +++ b/sensing/autoware_imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -18,6 +18,8 @@ #include +#include + namespace autoware::imu_corrector { class GyroBiasEstimationModuleTest : public ::testing::Test diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/crop_box_filter/crop_box_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/crop_box_filter/crop_box_filter_node.hpp index 2a983835e4e55..2eb0f2505642a 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/crop_box_filter/crop_box_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/crop_box_filter/crop_box_filter_node.hpp @@ -67,8 +67,8 @@ namespace autoware::pointcloud_preprocessor class CropBoxFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); + void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform // to new API diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp index a19c0c1e257b2..c898cbacf33ea 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp @@ -44,8 +44,8 @@ class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Fil using InputPointType = autoware::point_types::PointXYZIRCAEDT; using OutputPointType = autoware::point_types::PointXYZIRC; - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); + void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform // to new API diff --git a/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml index f4375e5a5cd2f..8912847ce1e4a 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml @@ -4,10 +4,8 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/crop_box_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/crop_box_filter_node.launch.xml index bb07daf8841b9..4cb9c9412df00 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/crop_box_filter_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/crop_box_filter_node.launch.xml @@ -3,11 +3,9 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml index d0293ca140e4f..5c79787a58e9e 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml @@ -4,11 +4,9 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/pickup_based_voxel_grid_downsample_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/pickup_based_voxel_grid_downsample_filter_node.launch.xml index 7c035cbcf1b52..83c0fe9a20d4c 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/pickup_based_voxel_grid_downsample_filter_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/pickup_based_voxel_grid_downsample_filter_node.launch.xml @@ -4,10 +4,8 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_accumulator_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_accumulator_node.launch.xml index 3f132a1586a36..1305ce5992866 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_accumulator_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_accumulator_node.launch.xml @@ -4,10 +4,8 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/radius_search_2d_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/radius_search_2d_outlier_filter_node.launch.xml index 665c8d7c6e37c..cdd1f8589f3e3 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/radius_search_2d_outlier_filter_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/radius_search_2d_outlier_filter_node.launch.xml @@ -4,10 +4,8 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter_node.launch.xml index 4663cece6ea60..12109ffec5b38 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter_node.launch.xml @@ -4,11 +4,9 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml index 68076f5c9c321..df45e81653303 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml @@ -3,11 +3,9 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml index 800fa5f3ec3ee..36f30fc2d41ee 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml @@ -4,11 +4,9 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/vector_map_inside_area_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/vector_map_inside_area_filter_node.launch.xml index 795234e185cdd..f5980b0814edc 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/vector_map_inside_area_filter_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/vector_map_inside_area_filter_node.launch.xml @@ -3,10 +3,8 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_downsample_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_downsample_filter_node.launch.xml index f674e8d9dfa14..90a2b9993fc18 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_downsample_filter_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_downsample_filter_node.launch.xml @@ -4,10 +4,8 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml index 380a46eed3159..e1b963922a2ff 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml @@ -4,10 +4,8 @@ - - diff --git a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp index 26be762acca43..5021a3551c939 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp @@ -18,6 +18,8 @@ #include #include +#include +#include namespace autoware::pointcloud_preprocessor { diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index a67c9f7f018e1..f8baae3405873 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -58,6 +58,7 @@ #include #include +#include #include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp index c1b3605c8b97c..ecbc5b8fd13ef 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp @@ -53,6 +53,7 @@ #include +#include #include namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 43a44f836b61a..8391894a4ada6 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -20,6 +20,10 @@ #include #include +#include +#include +#include + namespace autoware::pointcloud_preprocessor { diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 896c7fe563e64..9eaafeae9dbc7 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -16,6 +16,10 @@ #include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" +#include +#include +#include + namespace autoware::pointcloud_preprocessor { /** @brief Constructor. */ diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index 9a01ba3ddc1b1..804a91591271d 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -15,6 +15,7 @@ #include "autoware/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" #include +#include namespace autoware::pointcloud_preprocessor { diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp index e3c6af7433dda..10e2fa2511478 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp @@ -16,6 +16,9 @@ #include "robin_hood.h" +#include +#include + namespace { /** diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h index dd5cc9d60dd76..55307a2aa5552 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h @@ -68,6 +68,7 @@ #include #include #include +#include #if __cplusplus >= 201703L #include #endif @@ -85,7 +86,7 @@ #ifdef ROBIN_HOOD_TRACE_ENABLED #include #define ROBIN_HOOD_TRACE(...) \ - std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; + std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; // NOLINT #else #define ROBIN_HOOD_TRACE(x) #endif diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index 59000d71b8ad6..fae9043143ba4 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include namespace autoware::pointcloud_preprocessor { diff --git a/sensing/autoware_pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp b/sensing/autoware_pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp index 71b4c2026396e..e4bac9e35c248 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp @@ -14,6 +14,8 @@ #include "autoware/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp" +#include + namespace autoware::pointcloud_preprocessor { PolygonRemoverComponent::PolygonRemoverComponent(const rclcpp::NodeOptions & options) diff --git a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp index 5bf5069f1200f..42170fd88ee36 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp @@ -28,6 +28,7 @@ #include #include +#include #include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/src/utility/geometry.cpp b/sensing/autoware_pointcloud_preprocessor/src/utility/geometry.cpp index 7e9604c3dd305..1d08df944ca18 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/utility/geometry.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/utility/geometry.cpp @@ -14,6 +14,8 @@ #include "autoware/pointcloud_preprocessor/utility/geometry.hpp" +#include + namespace autoware::pointcloud_preprocessor::utils { void to_cgal_polygon(const geometry_msgs::msg::Polygon & polygon_in, PolygonCgal & polygon_out) diff --git a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter_node.cpp index 6fdf7791601cf..310b86526193c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter_node.cpp @@ -14,6 +14,10 @@ #include "autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter_node.hpp" +#include +#include +#include + namespace { autoware::universe_utils::Box2d calcBoundingBox( diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp index 895061229a994..aba3c354473eb 100644 --- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp @@ -41,6 +41,10 @@ #include #include +#include +#include +#include +#include enum AngleCoordinateSystem { HESAI, VELODYNE, CARTESIAN }; class DistortionCorrectorTest : public ::testing::Test diff --git a/sensing/autoware_radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp b/sensing/autoware_radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp index ea2dfc28b5a4c..e8551ae922f19 100644 --- a/sensing/autoware_radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp +++ b/sensing/autoware_radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp @@ -18,6 +18,8 @@ #include +#include + std::shared_ptr get_node( float velocity_y_threshold) { diff --git a/sensing/vehicle_velocity_converter/CHANGELOG.rst b/sensing/autoware_vehicle_velocity_converter/CHANGELOG.rst similarity index 100% rename from sensing/vehicle_velocity_converter/CHANGELOG.rst rename to sensing/autoware_vehicle_velocity_converter/CHANGELOG.rst diff --git a/sensing/vehicle_velocity_converter/CMakeLists.txt b/sensing/autoware_vehicle_velocity_converter/CMakeLists.txt similarity index 75% rename from sensing/vehicle_velocity_converter/CMakeLists.txt rename to sensing/autoware_vehicle_velocity_converter/CMakeLists.txt index bb50fbff90c4b..9ff990b928f91 100644 --- a/sensing/vehicle_velocity_converter/CMakeLists.txt +++ b/sensing/autoware_vehicle_velocity_converter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(vehicle_velocity_converter) +project(autoware_vehicle_velocity_converter) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "VehicleVelocityConverter" + PLUGIN "autoware::vehicle_velocity_converter::VehicleVelocityConverter" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR SingleThreadedExecutor ) diff --git a/sensing/vehicle_velocity_converter/README.md b/sensing/autoware_vehicle_velocity_converter/README.md similarity index 97% rename from sensing/vehicle_velocity_converter/README.md rename to sensing/autoware_vehicle_velocity_converter/README.md index 3c7292f3fcdc4..f398a72ea824d 100644 --- a/sensing/vehicle_velocity_converter/README.md +++ b/sensing/autoware_vehicle_velocity_converter/README.md @@ -1,4 +1,4 @@ -# vehicle_velocity_converter +# autoware_vehicle_velocity_converter ## Purpose diff --git a/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml b/sensing/autoware_vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml similarity index 100% rename from sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml rename to sensing/autoware_vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml diff --git a/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml b/sensing/autoware_vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml similarity index 60% rename from sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml rename to sensing/autoware_vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml index 84e1838dc89eb..d8386e3b80820 100644 --- a/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml +++ b/sensing/autoware_vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/sensing/vehicle_velocity_converter/package.xml b/sensing/autoware_vehicle_velocity_converter/package.xml similarity index 84% rename from sensing/vehicle_velocity_converter/package.xml rename to sensing/autoware_vehicle_velocity_converter/package.xml index 57400454b9d8e..323002396c841 100644 --- a/sensing/vehicle_velocity_converter/package.xml +++ b/sensing/autoware_vehicle_velocity_converter/package.xml @@ -1,9 +1,9 @@ - vehicle_velocity_converter + autoware_vehicle_velocity_converter 0.38.0 - The vehicle_velocity_converter package + The autoware_vehicle_velocity_converter package Ryu Yamamoto Apache License 2.0 diff --git a/sensing/vehicle_velocity_converter/schema/vehicle_velocity_converter.json b/sensing/autoware_vehicle_velocity_converter/schema/vehicle_velocity_converter.json similarity index 100% rename from sensing/vehicle_velocity_converter/schema/vehicle_velocity_converter.json rename to sensing/autoware_vehicle_velocity_converter/schema/vehicle_velocity_converter.json diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/autoware_vehicle_velocity_converter/src/vehicle_velocity_converter.cpp similarity index 91% rename from sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp rename to sensing/autoware_vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index c382f0f16f6e5..2211fa0cbb03e 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/autoware_vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "vehicle_velocity_converter/vehicle_velocity_converter.hpp" +#include "vehicle_velocity_converter.hpp" +#include + +namespace autoware::vehicle_velocity_converter +{ VehicleVelocityConverter::VehicleVelocityConverter(const rclcpp::NodeOptions & options) : rclcpp::Node("vehicle_velocity_converter", options) { @@ -53,6 +57,7 @@ void VehicleVelocityConverter::callback_velocity_report( twist_with_covariance_pub_->publish(twist_with_covariance_msg); } +} // namespace autoware::vehicle_velocity_converter #include -RCLCPP_COMPONENTS_REGISTER_NODE(VehicleVelocityConverter) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::vehicle_velocity_converter::VehicleVelocityConverter) diff --git a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/sensing/autoware_vehicle_velocity_converter/src/vehicle_velocity_converter.hpp similarity index 86% rename from sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp rename to sensing/autoware_vehicle_velocity_converter/src/vehicle_velocity_converter.hpp index 5f7b1c044d7e3..9d65e34c4457c 100644 --- a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp +++ b/sensing/autoware_vehicle_velocity_converter/src/vehicle_velocity_converter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef VEHICLE_VELOCITY_CONVERTER__VEHICLE_VELOCITY_CONVERTER_HPP_ -#define VEHICLE_VELOCITY_CONVERTER__VEHICLE_VELOCITY_CONVERTER_HPP_ +#ifndef VEHICLE_VELOCITY_CONVERTER_HPP_ +#define VEHICLE_VELOCITY_CONVERTER_HPP_ #include @@ -25,6 +25,8 @@ #include #include +namespace autoware::vehicle_velocity_converter +{ class VehicleVelocityConverter : public rclcpp::Node { public: @@ -43,5 +45,6 @@ class VehicleVelocityConverter : public rclcpp::Node double stddev_wz_; double speed_scale_factor_; }; +} // namespace autoware::vehicle_velocity_converter -#endif // VEHICLE_VELOCITY_CONVERTER__VEHICLE_VELOCITY_CONVERTER_HPP_ +#endif // VEHICLE_VELOCITY_CONVERTER_HPP_ diff --git a/setup.cfg b/setup.cfg index 5214751c7ba6b..4d7d5e5b959d9 100644 --- a/setup.cfg +++ b/setup.cfg @@ -1,3 +1,7 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + [flake8] # Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_flake8/ament_flake8/configuration/ament_flake8.ini extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,CNL100,E203,E501,Q000 diff --git a/simulator/autoware_carla_interface/README.md b/simulator/autoware_carla_interface/README.md index a44cb4708b5c3..c98d39bb4f121 100644 --- a/simulator/autoware_carla_interface/README.md +++ b/simulator/autoware_carla_interface/README.md @@ -24,7 +24,7 @@ This ros package enables communication between Autoware and CARLA for autonomous 1. Download maps (y-axis inverted version) to arbitrary location 2. Change names and create the map folder (example: Town01) inside `autoware_map`. (`point_cloud/Town01.pcd` -> `autoware_map/Town01/pointcloud_map.pcd`, `vector_maps/lanelet2/Town01.osm`-> `autoware_map/Town01/lanelet2_map.osm`) -3. Create `map_projector_info.yaml` on the folder and add `projector_type: local` on the first line. +3. Create `map_projector_info.yaml` on the folder and add `projector_type: Local` on the first line. ### Build @@ -136,14 +136,14 @@ The maps provided by the Carla Simulator ([Carla Lanelet2 Maps](https://bitbucke - Options to Modify the Map - A. Create a New Map from Scratch - - Use the [Tier4 Vector Map Builder](https://tools.tier4.jp/feature/vector_map_builder_ll2/) to create a new map. + - Use the [TIER IV Vector Map Builder](https://tools.tier4.jp/feature/vector_map_builder_ll2/) to create a new map. - B. Modify the Existing Carla Lanelet2 Maps - Adjust the longitude and latitude of the [Carla Lanelet2 Maps](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/) to align with the PCD (origin). - Use this [tool](https://github.com/mraditya01/offset_lanelet2/tree/main) to modify the coordinates. - - Snap Lanelet with PCD and add the traffic lights using the [Tier4 Vector Map Builder](https://tools.tier4.jp/feature/vector_map_builder_ll2/). + - Snap Lanelet with PCD and add the traffic lights using the [TIER IV Vector Map Builder](https://tools.tier4.jp/feature/vector_map_builder_ll2/). -- When using the Tier4 Vector Map Builder, you must convert the PCD format from `binary_compressed` to `ascii`. You can use `pcl_tools` for this conversion. +- When using the TIER IV Vector Map Builder, you must convert the PCD format from `binary_compressed` to `ascii`. You can use `pcl_tools` for this conversion. - For reference, an example of Town01 with added traffic lights at one intersection can be downloaded [here](https://drive.google.com/drive/folders/1QFU0p3C8NW71sT5wwdnCKXoZFQJzXfTG?usp=sharing). ## Tips diff --git a/simulator/autoware_carla_interface/setup.py b/simulator/autoware_carla_interface/setup.py index 346f1838670ab..84f7a544555a2 100644 --- a/simulator/autoware_carla_interface/setup.py +++ b/simulator/autoware_carla_interface/setup.py @@ -1,6 +1,7 @@ from glob import glob import os +from setuptools import find_packages from setuptools import setup package_name = "autoware_carla_interface" @@ -8,13 +9,13 @@ setup( name=package_name, version="0.38.0", - packages=[package_name], + packages=find_packages(where="src"), data_files=[ - ("share/" + package_name, glob("config/*")), - ("share/" + package_name, glob("calibration_maps/*.csv")), - ("share/" + package_name, ["package.xml"]), - (os.path.join("share", package_name), glob("launch/autoware_carla_interface.launch.xml")), - ("share/ament_index/resource_index/packages", ["resource/autoware_carla_interface"]), + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (os.path.join("share", package_name), ["package.xml"]), + (os.path.join("share", package_name), glob("config/*")), + (os.path.join("share", package_name), glob("calibration_maps/*.csv")), + (os.path.join("share", package_name), glob("launch/*.launch.xml")), ], install_requires=["setuptools"], zip_safe=True, 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 4d50e547fc759..cf46ecddf516f 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -124,7 +124,6 @@ class DummyPerceptionPublisherNode : public rclcpp::Node double detection_successful_rate_; bool enable_ray_tracing_; bool use_object_recognition_; - bool use_real_param_; bool use_base_link_z_; bool publish_ground_truth_objects_; std::unique_ptr pointcloud_creator_; diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index dba230f23f108..d72b16d303cec 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -20,6 +20,8 @@ #include #include #include + +#include #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp index 05354fa0a9663..977336f63eee3 100644 --- a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp +++ b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp @@ -24,6 +24,7 @@ #include #include #include +#include namespace { diff --git a/simulator/dummy_perception_publisher/src/signed_distance_function.cpp b/simulator/dummy_perception_publisher/src/signed_distance_function.cpp index e20c19da93af0..01bef1851b25e 100644 --- a/simulator/dummy_perception_publisher/src/signed_distance_function.cpp +++ b/simulator/dummy_perception_publisher/src/signed_distance_function.cpp @@ -16,6 +16,7 @@ #include +#include #include #include diff --git a/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp b/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp index 948c2b45f6615..0a71c1681d1d3 100644 --- a/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp +++ b/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp @@ -51,6 +51,7 @@ #include +#include #include #include #include diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/submodel_interface.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/submodel_interface.hpp index d4662036709d5..1cbad103278fb 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/submodel_interface.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/submodel_interface.hpp @@ -20,6 +20,8 @@ class SubModelInterface { public: + virtual ~SubModelInterface() = default; + /** * @brief set time step of the model * @param [in] dt time step diff --git a/simulator/learning_based_vehicle_model/src/interconnected_model.cpp b/simulator/learning_based_vehicle_model/src/interconnected_model.cpp index 4dd3b321312d0..d2fef15aa4e88 100644 --- a/simulator/learning_based_vehicle_model/src/interconnected_model.cpp +++ b/simulator/learning_based_vehicle_model/src/interconnected_model.cpp @@ -14,6 +14,10 @@ #include "learning_based_vehicle_model/interconnected_model.hpp" +#include +#include +#include + void InterconnectedModel::mapInputs(std::vector in_names) { // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index diff --git a/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp b/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp index c7944116d1758..b300f3837b18a 100644 --- a/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp +++ b/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp @@ -14,6 +14,8 @@ #include "learning_based_vehicle_model/model_connections_helpers.hpp" +#include + std::vector fillVectorUsingMap( std::vector vector1, std::vector vector2, const std::vector & map, bool inverse) diff --git a/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp b/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp index 9c3cb9aa06150..10581179baf4a 100644 --- a/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp +++ b/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp @@ -17,6 +17,7 @@ #include #include +#include #include namespace py = pybind11; 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 5ca886b1b5847..bab4531484aa6 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 @@ -50,7 +50,7 @@ class SimModelInterface /** * @brief destructor */ - ~SimModelInterface() = default; + virtual ~SimModelInterface() = default; /** * @brief get state vector of model 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 685708844d32d..d61654c32af02 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 @@ -35,9 +35,11 @@ #include #include #include +#include #include #include #include +#include using namespace std::literals::chrono_literals; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp index b410f089725a2..fa1f7978a798c 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp @@ -15,8 +15,10 @@ #include "simple_planning_simulator/utils/csv_loader.hpp" #include +#include #include #include +#include #include CSVLoader::CSVLoader(const std::string & csv_path) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp index 469c30dc4f0cf..bfcf571d8c09b 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -17,6 +17,8 @@ #include "autoware_vehicle_msgs/msg/gear_command.hpp" #include +#include +#include bool ActuationMap::readActuationMapFromCSV(const std::string & csv_path, const bool validation) { 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 b987f74db0436..ffef5ab7b236c 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 @@ -17,6 +17,7 @@ #include "autoware_vehicle_msgs/msg/gear_command.hpp" #include +#include SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp index fba34e04ade7c..4ddcc0e498b70 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp @@ -17,6 +17,9 @@ #include "learning_based_vehicle_model/interconnected_model.hpp" #include +#include +#include +#include SimModelLearnedSteerVel::SimModelLearnedSteerVel( double dt, std::vector model_python_paths, 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 6f2590417e574..0baa3302f86c7 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -19,6 +19,11 @@ #include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" +#include +#include +#include +#include + #ifdef ROS_DISTRO_GALACTIC #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #else diff --git a/simulator/tier4_dummy_object_rviz_plugin/src/tools/interactive_object.cpp b/simulator/tier4_dummy_object_rviz_plugin/src/tools/interactive_object.cpp index 83852d0c85011..1daff9459f447 100644 --- a/simulator/tier4_dummy_object_rviz_plugin/src/tools/interactive_object.cpp +++ b/simulator/tier4_dummy_object_rviz_plugin/src/tools/interactive_object.cpp @@ -55,6 +55,7 @@ #include #include #include +#include namespace rviz_plugins { diff --git a/system/autoware_component_monitor/src/component_monitor_node.cpp b/system/autoware_component_monitor/src/component_monitor_node.cpp index 3c5d6b6667725..ab234f7fd66f2 100644 --- a/system/autoware_component_monitor/src/component_monitor_node.cpp +++ b/system/autoware_component_monitor/src/component_monitor_node.cpp @@ -147,6 +147,7 @@ std::uint64_t ComponentMonitor::parse_memory_res(const std::string & mem_res) { // example 1: 12.3g // example 2: 123 (without suffix, just bytes) + // NOLINTNEXTLINE(readability/casting) static const std::unordered_map> unit_map{ {'k', unit_conversions::kib_to_bytes}, {'m', unit_conversions::mib_to_bytes}, {'g', unit_conversions::gib_to_bytes}, {'t', unit_conversions::tib_to_bytes}, diff --git a/system/autoware_default_adapi/src/perception.cpp b/system/autoware_default_adapi/src/perception.cpp index e764af4ca816a..3e858e7763baf 100644 --- a/system/autoware_default_adapi/src/perception.cpp +++ b/system/autoware_default_adapi/src/perception.cpp @@ -14,6 +14,7 @@ #include "perception.hpp" +#include #include namespace autoware::default_adapi diff --git a/system/autoware_default_adapi/src/vehicle.cpp b/system/autoware_default_adapi/src/vehicle.cpp index 5620062ede278..fc35efff523c2 100644 --- a/system/autoware_default_adapi/src/vehicle.cpp +++ b/system/autoware_default_adapi/src/vehicle.cpp @@ -20,6 +20,7 @@ #include #include +#include namespace autoware::default_adapi { diff --git a/system/autoware_processing_time_checker/README.md b/system/autoware_processing_time_checker/README.md index 5745ec52086cc..1cb6a289bc54f 100644 --- a/system/autoware_processing_time_checker/README.md +++ b/system/autoware_processing_time_checker/README.md @@ -31,6 +31,8 @@ ros2 launch autoware_processing_time_checker processing_time_checker.launch.xml {{ json_to_markdown("system/autoware_processing_time_checker/schema/processing_time_checker.schema.json") }} +If `output_metrics = true`, the node writes the statics of the processing_time measured during its lifetime to `/autoware_metrics/-.json` when shut down. + ## Assumptions / Known limits TBD. diff --git a/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml index 033b20d234fd9..526e413ea196f 100644 --- a/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml +++ b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml @@ -2,6 +2,7 @@ ros__parameters: update_rate: 10.0 processing_time_topic_name_list: + - /control/control_evaluator/debug/processing_time_ms - /control/control_validator/debug/processing_time_ms - /control/trajectory_follower/controller_node_exe/lateral/debug/processing_time_ms - /control/trajectory_follower/controller_node_exe/longitudinal/debug/processing_time_ms @@ -9,6 +10,9 @@ - /control/vehicle_cmd_gate/debug/processing_time_ms - /perception/object_recognition/prediction/map_based_prediction/debug/processing_time_ms - /perception/object_recognition/tracking/multi_object_tracker/debug/processing_time_ms + - /planning/mission_planning/mission_planner/debug/processing_time_ms + - /planning/mission_planning/route_selector/debug/processing_time_ms + - /planning/planning_evaluator/debug/processing_time_ms - /planning/planning_validator/debug/processing_time_ms - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance_by_lane_change/processing_time_ms - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/dynamic_obstacle_avoidance/processing_time_ms @@ -37,6 +41,7 @@ - /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/processing_time_ms - /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/processing_time_ms - /planning/scenario_planning/parking/costmap_generator/debug/processing_time_ms + - /planning/scenario_planning/parking/freespace_planner/debug/processing_time_ms - /planning/scenario_planning/scenario_selector/debug/processing_time_ms - /planning/scenario_planning/velocity_smoother/debug/processing_time_ms - /simulation/shape_estimation/debug/processing_time_ms diff --git a/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml b/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml index 1e9eb6a3d03e1..9789bd39dca94 100644 --- a/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml +++ b/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml @@ -1,7 +1,10 @@ + + + diff --git a/system/autoware_processing_time_checker/package.xml b/system/autoware_processing_time_checker/package.xml index 73a0b43e44c50..16b225f3ef425 100644 --- a/system/autoware_processing_time_checker/package.xml +++ b/system/autoware_processing_time_checker/package.xml @@ -12,6 +12,8 @@ ament_cmake autoware_cmake + autoware_universe_utils + nlohmann-json-dev rclcpp rclcpp_components tier4_debug_msgs diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.cpp b/system/autoware_processing_time_checker/src/processing_time_checker.cpp index 3ab96ab0f9711..1e3f04af8fa89 100644 --- a/system/autoware_processing_time_checker/src/processing_time_checker.cpp +++ b/system/autoware_processing_time_checker/src/processing_time_checker.cpp @@ -14,8 +14,12 @@ #include "processing_time_checker.hpp" +#include #include +#include +#include +#include #include #include @@ -38,6 +42,7 @@ std::string get_last_name(const std::string & str) ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_options) : Node("processing_time_checker", node_options) { + output_metrics_ = declare_parameter("output_metrics"); const double update_rate = declare_parameter("update_rate"); const auto processing_time_topic_name_list = declare_parameter>("processing_time_topic_name_list"); @@ -47,7 +52,7 @@ ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_op // extract module name from topic name auto tmp_topic_name = processing_time_topic_name; - for (size_t i = 0; i < 4; ++i) { // 4 is enouh for the search depth + for (size_t i = 0; i < 4; ++i) { // 4 is enough for the search depth tmp_topic_name = remove_last_name(tmp_topic_name); const auto module_name_candidate = get_last_name(tmp_topic_name); // clang-format off @@ -64,6 +69,7 @@ ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_op // register module name if (module_name) { module_name_map_.insert_or_assign(processing_time_topic_name, *module_name); + processing_time_accumulator_map_.insert_or_assign(*module_name, Accumulator()); } else { throw std::invalid_argument("The format of the processing time topic name is not correct."); } @@ -79,6 +85,7 @@ ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_op processing_time_topic_name, 1, [this, &module_name]([[maybe_unused]] const Float64Stamped & msg) { processing_time_map_.insert_or_assign(module_name, msg.data); + processing_time_accumulator_map_.at(module_name).add(msg.data); })); // clang-format on } @@ -90,6 +97,54 @@ ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_op this, get_clock(), period_ns, std::bind(&ProcessingTimeChecker::on_timer, this)); } +ProcessingTimeChecker::~ProcessingTimeChecker() +{ + if (!output_metrics_) { + return; + } + + // generate json data + nlohmann::json j; + for (const auto & accumulator_iterator : processing_time_accumulator_map_) { + const auto module_name = accumulator_iterator.first; + const auto processing_time_accumulator = accumulator_iterator.second; + j[module_name + "/min"] = processing_time_accumulator.min(); + j[module_name + "/max"] = processing_time_accumulator.max(); + j[module_name + "/mean"] = processing_time_accumulator.mean(); + j[module_name + "/count"] = processing_time_accumulator.count(); + j[module_name + "/description"] = "processing time of " + module_name + "[ms]"; + } + + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } + } + + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_processing_time_checker-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } +} + void ProcessingTimeChecker::on_timer() { // create MetricArrayMsg diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.hpp b/system/autoware_processing_time_checker/src/processing_time_checker.hpp index 199410623f8b1..77450923509f2 100644 --- a/system/autoware_processing_time_checker/src/processing_time_checker.hpp +++ b/system/autoware_processing_time_checker/src/processing_time_checker.hpp @@ -15,6 +15,8 @@ #ifndef PROCESSING_TIME_CHECKER_HPP_ #define PROCESSING_TIME_CHECKER_HPP_ +#include "autoware/universe_utils/math/accumulator.hpp" + #include #include @@ -27,6 +29,7 @@ namespace autoware::processing_time_checker { +using autoware::universe_utils::Accumulator; using MetricMsg = tier4_metric_msgs::msg::Metric; using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; using tier4_debug_msgs::msg::Float64Stamped; @@ -35,6 +38,7 @@ class ProcessingTimeChecker : public rclcpp::Node { public: explicit ProcessingTimeChecker(const rclcpp::NodeOptions & node_options); + ~ProcessingTimeChecker() override; private: void on_timer(); @@ -44,10 +48,15 @@ class ProcessingTimeChecker : public rclcpp::Node rclcpp::Publisher::SharedPtr metrics_pub_; std::vector::SharedPtr> processing_time_subscribers_; + // parameters + bool output_metrics_; + // topic name - module name std::unordered_map module_name_map_{}; // module name - processing time std::unordered_map processing_time_map_{}; + // module name - accumulator + std::unordered_map> processing_time_accumulator_map_{}; }; } // namespace autoware::processing_time_checker diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp index f20c763d643ce..827a7e3060acc 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #define FMT_HEADER_ONLY #include diff --git a/system/bluetooth_monitor/service/main.cpp b/system/bluetooth_monitor/service/main.cpp index a3ccb971a5eb7..d3aef2c0696dd 100644 --- a/system/bluetooth_monitor/service/main.cpp +++ b/system/bluetooth_monitor/service/main.cpp @@ -20,6 +20,8 @@ #include #include +#include + /** * @brief print usage */ diff --git a/system/component_state_monitor/src/main.cpp b/system/component_state_monitor/src/main.cpp index 2a05ef1a38911..c87963b1b21c0 100644 --- a/system/component_state_monitor/src/main.cpp +++ b/system/component_state_monitor/src/main.cpp @@ -14,6 +14,8 @@ #include "main.hpp" +#include +#include #include namespace component_state_monitor diff --git a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp index b91b777831368..e622d089109f4 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp @@ -23,9 +23,11 @@ #include #include #include +#include #include #include #include +#include namespace diagnostic_graph_aggregator { diff --git a/system/diagnostic_graph_aggregator/src/common/graph/data.cpp b/system/diagnostic_graph_aggregator/src/common/graph/data.cpp index 2a4aef7e39c1f..e1d053e6d259c 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/data.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/data.cpp @@ -14,6 +14,8 @@ #include "data.hpp" +#include + namespace diagnostic_graph_aggregator { diff --git a/system/diagnostic_graph_aggregator/src/common/graph/error.cpp b/system/diagnostic_graph_aggregator/src/common/graph/error.cpp index e78a444c5ab7f..7a4336e62d8a3 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/error.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/error.cpp @@ -14,6 +14,8 @@ #include "error.hpp" +#include + namespace diagnostic_graph_aggregator { diff --git a/system/diagnostic_graph_aggregator/src/common/graph/graph.cpp b/system/diagnostic_graph_aggregator/src/common/graph/graph.cpp index 0a7d78a5ce982..7977209b4905c 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/graph.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/graph.cpp @@ -19,6 +19,7 @@ #include "loader.hpp" #include "units.hpp" +#include #include namespace diagnostic_graph_aggregator diff --git a/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp b/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp index e4d2e470a0d32..5edef61340fe8 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp @@ -20,7 +20,11 @@ #include "types.hpp" #include "units.hpp" +#include +#include +#include #include +#include namespace diagnostic_graph_aggregator { diff --git a/system/diagnostic_graph_aggregator/src/tool/plantuml.cpp b/system/diagnostic_graph_aggregator/src/tool/plantuml.cpp index 9f7eb34bf3454..68c0082908430 100644 --- a/system/diagnostic_graph_aggregator/src/tool/plantuml.cpp +++ b/system/diagnostic_graph_aggregator/src/tool/plantuml.cpp @@ -16,6 +16,7 @@ #include "graph/units.hpp" #include +#include namespace diagnostic_graph_aggregator { diff --git a/system/diagnostic_graph_aggregator/src/tool/tree.cpp b/system/diagnostic_graph_aggregator/src/tool/tree.cpp index 1e6fe93b18a80..f367ec2113808 100644 --- a/system/diagnostic_graph_aggregator/src/tool/tree.cpp +++ b/system/diagnostic_graph_aggregator/src/tool/tree.cpp @@ -16,6 +16,7 @@ #include "graph/units.hpp" #include +#include namespace diagnostic_graph_aggregator { diff --git a/system/diagnostic_graph_aggregator/test/src/test2.cpp b/system/diagnostic_graph_aggregator/test/src/test2.cpp index b010994f5a1ca..4a0199a89f37e 100644 --- a/system/diagnostic_graph_aggregator/test/src/test2.cpp +++ b/system/diagnostic_graph_aggregator/test/src/test2.cpp @@ -22,6 +22,9 @@ #include +#include +#include + using namespace diagnostic_graph_aggregator; // NOLINT(build/namespaces) using diagnostic_msgs::msg::DiagnosticArray; diff --git a/system/diagnostic_graph_aggregator/test/src/utils.cpp b/system/diagnostic_graph_aggregator/test/src/utils.cpp index c64812afa649a..f92a414e2a678 100644 --- a/system/diagnostic_graph_aggregator/test/src/utils.cpp +++ b/system/diagnostic_graph_aggregator/test/src/utils.cpp @@ -14,6 +14,8 @@ #include "utils.hpp" +#include + std::filesystem::path resource(const std::string & path) { return std::filesystem::path(TEST_RESOURCE_PATH) / path; 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 c5b386dbe2c86..275e4ffcb1c7e 100644 --- a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp +++ b/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp @@ -37,6 +37,8 @@ class DiagUnit using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; using DiagnosticLevel = DiagnosticStatus::_level_type; + virtual ~DiagUnit() = default; + struct DiagChild { DiagLink * link; diff --git a/system/diagnostic_graph_utils/src/lib/graph.cpp b/system/diagnostic_graph_utils/src/lib/graph.cpp index 423e1b40be12e..007b42547bee1 100644 --- a/system/diagnostic_graph_utils/src/lib/graph.cpp +++ b/system/diagnostic_graph_utils/src/lib/graph.cpp @@ -14,6 +14,8 @@ #include "diagnostic_graph_utils/graph.hpp" +#include + namespace diagnostic_graph_utils { diff --git a/system/diagnostic_graph_utils/src/lib/subscription.cpp b/system/diagnostic_graph_utils/src/lib/subscription.cpp index 655544c2e1f28..c10481ef8f16e 100644 --- a/system/diagnostic_graph_utils/src/lib/subscription.cpp +++ b/system/diagnostic_graph_utils/src/lib/subscription.cpp @@ -14,6 +14,8 @@ #include "diagnostic_graph_utils/subscription.hpp" +#include + namespace diagnostic_graph_utils { diff --git a/system/diagnostic_graph_utils/src/node/dump.cpp b/system/diagnostic_graph_utils/src/node/dump.cpp index 8456a92cbaa9b..42c66224b2c37 100644 --- a/system/diagnostic_graph_utils/src/node/dump.cpp +++ b/system/diagnostic_graph_utils/src/node/dump.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include namespace diagnostic_graph_utils diff --git a/system/diagnostic_graph_utils/src/node/logging.cpp b/system/diagnostic_graph_utils/src/node/logging.cpp index 009af51e949cd..b166a087200a0 100644 --- a/system/diagnostic_graph_utils/src/node/logging.cpp +++ b/system/diagnostic_graph_utils/src/node/logging.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include namespace diagnostic_graph_utils 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 51a0846b45179..6bc1378507a37 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -14,6 +14,11 @@ #include "dummy_diag_publisher/dummy_diag_publisher_core.hpp" +#include +#include +#include +#include + #define FMT_HEADER_ONLY #include diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp index 5c698a0a01860..1bc14bf927835 100644 --- a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp index d92af2186f415..52cfef93aa522 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/system/hazard_status_converter/src/converter.cpp @@ -14,6 +14,7 @@ #include "converter.hpp" +#include #include #include diff --git a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp index 6b18555053582..fc703ab16e259 100644 --- a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp +++ b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp @@ -16,6 +16,8 @@ #include +#include + namespace mrm_comfortable_stop_operator { 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 5c1ca5e04de0e..9c941888a3545 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 @@ -16,6 +16,8 @@ #include +#include + namespace mrm_emergency_stop_operator { diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index 589f9a3970098..740f841382f47 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -330,10 +330,10 @@ int get_nvme_identify(int fd, HddInfo * info) char data[4096]{}; // Fixed size for Identify command // The Identify command returns a data buffer that describes information about the NVM subsystem - cmd.opcode = 0x06; // Identify - cmd.addr = (uint64_t)data; // memory address of data - cmd.data_len = sizeof(data); // length of data - cmd.cdw10 = 0x01; // Identify Controller data structure + cmd.opcode = 0x06; // Identify + cmd.addr = reinterpret_cast(data); // memory address of data + cmd.data_len = sizeof(data); // length of data + cmd.cdw10 = 0x01; // Identify Controller data structure // send Admin Command to device int ret = ioctl(fd, NVME_IOCTL_ADMIN_CMD, &cmd); @@ -368,13 +368,13 @@ int get_nvme_smart_data(int fd, HddInfo * info) unsigned char data[144]{}; // 36 Dword (get byte 0 to 143) // The Get Log Page command returns a data buffer containing the log page requested - cmd.opcode = 0x02; // Get Log Page - cmd.nsid = 0xFFFFFFFF; // Global log page - cmd.addr = (uint64_t)data; // memory address of data - cmd.data_len = sizeof(data); // length of data - cmd.cdw10 = 0x00240002; // Bit 27:16 Number of Dwords (NUMD) = 024h (36 Dword) - // - Minimum necessary size to obtain S.M.A.R.T. informations - // Bit 07:00 = 02h (SMART / Health Information) + cmd.opcode = 0x02; // Get Log Page + cmd.nsid = 0xFFFFFFFF; // Global log page + cmd.addr = reinterpret_cast(data); // memory address of data + cmd.data_len = sizeof(data); // length of data + cmd.cdw10 = 0x00240002; // Bit 27:16 Number of Dwords (NUMD) = 024h (36 Dword) + // - Minimum necessary size to obtain S.M.A.R.T. informations + // Bit 07:00 = 02h (SMART / Health Information) // send Admin Command to device int ret = ioctl(fd, NVME_IOCTL_ADMIN_CMD, &cmd); diff --git a/system/system_monitor/reader/msr_reader/msr_reader.cpp b/system/system_monitor/reader/msr_reader/msr_reader.cpp index 59a6edc91ace7..fc7bcab795be0 100644 --- a/system/system_monitor/reader/msr_reader/msr_reader.cpp +++ b/system/system_monitor/reader/msr_reader/msr_reader.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp index 940afdd9e5c35..cc0a8f81b6bff 100644 --- a/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp +++ b/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp @@ -18,7 +18,9 @@ #include #include +#include #include +#include /** * @brief print usage diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp index 927f1b175b46b..fe5df51946fc6 100644 --- a/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp +++ b/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp @@ -26,6 +26,11 @@ #include #include +#include +#include +#include +#include + namespace process = boost::process; namespace traffic_reader_service diff --git a/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp b/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp index 13d7151bb14c7..f824ad49df867 100644 --- a/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp +++ b/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp @@ -30,8 +30,10 @@ #include #include +#include #include #include +#include namespace bp = boost::process; namespace fs = boost::filesystem; diff --git a/system/system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp b/system/system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp index 0000d6ead1c6a..496a782add33e 100644 --- a/system/system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp +++ b/system/system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index 50ac4363da609..60e1c401a05f3 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -32,8 +32,10 @@ #include #include +#include #include #include +#include #include namespace bp = boost::process; diff --git a/system/system_monitor/src/mem_monitor/mem_monitor.cpp b/system/system_monitor/src/mem_monitor/mem_monitor.cpp index 4d141294c3fa5..1caa011e15bed 100644 --- a/system/system_monitor/src/mem_monitor/mem_monitor.cpp +++ b/system/system_monitor/src/mem_monitor/mem_monitor.cpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace bp = boost::process; diff --git a/system/system_monitor/src/net_monitor/net_monitor.cpp b/system/system_monitor/src/net_monitor/net_monitor.cpp index a074663a7ee7c..d99150f0b3037 100644 --- a/system/system_monitor/src/net_monitor/net_monitor.cpp +++ b/system/system_monitor/src/net_monitor/net_monitor.cpp @@ -26,6 +26,11 @@ #include #include +#include +#include +#include +#include + #define FMT_HEADER_ONLY #include #include diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp index 4e697fa18ec50..590793cf1f7fe 100644 --- a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -29,6 +29,7 @@ #include #include #include +#include namespace bp = boost::process; namespace fs = boost::filesystem; diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 73059d2d162fa..faaf1abf1c9d3 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include ProcessMonitor::ProcessMonitor(const rclcpp::NodeOptions & options) diff --git a/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp b/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp index fda75c21535cf..f7771fb9f47d3 100644 --- a/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp +++ b/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include namespace bp = boost::process; diff --git a/system/system_monitor/test/src/process_monitor/top3.cpp b/system/system_monitor/test/src/process_monitor/top3.cpp index 2725ee2676064..3400b608ed9b7 100644 --- a/system/system_monitor/test/src/process_monitor/top3.cpp +++ b/system/system_monitor/test/src/process_monitor/top3.cpp @@ -19,6 +19,8 @@ #include +#include + int main(int argc, char ** argv) { printf("Tasks:"); diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index e890a5208bf22..02743537d6bb3 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -15,7 +15,9 @@ #include "reaction_analyzer_node.hpp" #include +#include #include +#include namespace reaction_analyzer { diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp index a5d1be613ee5a..0e119c3fdcbf7 100644 --- a/tools/reaction_analyzer/src/subscriber.cpp +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -15,8 +15,11 @@ #include "subscriber.hpp" #include +#include #include +#include #include +#include namespace reaction_analyzer::subscriber { diff --git a/tools/reaction_analyzer/src/topic_publisher.cpp b/tools/reaction_analyzer/src/topic_publisher.cpp index f720786d422bc..c78faf7ad656e 100644 --- a/tools/reaction_analyzer/src/topic_publisher.cpp +++ b/tools/reaction_analyzer/src/topic_publisher.cpp @@ -15,7 +15,11 @@ #include "topic_publisher.hpp" #include +#include #include +#include +#include +#include namespace reaction_analyzer::topic_publisher { diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp index 107c1deb2f196..9075cdc4a77dd 100644 --- a/tools/reaction_analyzer/src/utils.cpp +++ b/tools/reaction_analyzer/src/utils.cpp @@ -14,6 +14,12 @@ #include "utils.hpp" +#include +#include +#include +#include +#include + namespace reaction_analyzer { SubscriberMessageType get_subscriber_message_type(const std::string & message_type) diff --git a/vehicle/autoware_accel_brake_map_calibrator/README.md b/vehicle/autoware_accel_brake_map_calibrator/README.md index 024c8059a169e..a85e7de98a3ef 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/README.md +++ b/vehicle/autoware_accel_brake_map_calibrator/README.md @@ -212,10 +212,11 @@ Update by Recursive Least Squares(RLS) method using data close enough to each gr #### Parameters Data selection is determined by the following thresholds. -| Name | Default Value | + +| Name | Default Value | | ----------------------- | ------------- | -| velocity_diff_threshold | 0.556 | -| pedal_diff_threshold | 0.03 | +| velocity_diff_threshold | 0.556 | +| pedal_diff_threshold | 0.03 | #### Update formula diff --git a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 39d81c26961bb..2a0ad4f987dad 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -47,6 +47,7 @@ #include "tier4_vehicle_msgs/srv/update_accel_brake_map.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include #include #include #include diff --git a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 3750fdb77b258..9d7de55aff54d 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -19,6 +19,7 @@ #include "rclcpp/logging.hpp" #include +#include #include #include #include diff --git a/vehicle/autoware_external_cmd_converter/src/node.cpp b/vehicle/autoware_external_cmd_converter/src/node.cpp index 0f7166c71fbff..267836b91f945 100644 --- a/vehicle/autoware_external_cmd_converter/src/node.cpp +++ b/vehicle/autoware_external_cmd_converter/src/node.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include diff --git a/vehicle/autoware_external_cmd_converter/tests/test_external_cmd_converter.cpp b/vehicle/autoware_external_cmd_converter/tests/test_external_cmd_converter.cpp index a508359af60e8..1fc037592bb08 100644 --- a/vehicle/autoware_external_cmd_converter/tests/test_external_cmd_converter.cpp +++ b/vehicle/autoware_external_cmd_converter/tests/test_external_cmd_converter.cpp @@ -24,6 +24,7 @@ #include #include +#include using autoware::external_cmd_converter::ExternalCmdConverterNode; using nav_msgs::msg::Odometry; using tier4_control_msgs::msg::GateMode; diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp index 0a31e2089e4ef..999b52e1a1c81 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -15,7 +15,9 @@ #include "autoware_raw_vehicle_cmd_converter/csv_loader.hpp" #include +#include #include +#include #include namespace autoware::raw_vehicle_cmd_converter diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp index 362fb53eba67d..e4cdbe60fd4cd 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp @@ -15,6 +15,7 @@ #include "autoware_raw_vehicle_cmd_converter/node.hpp" #include +#include #include #include #include diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp index 6b2bffa7630f9..d41f4facf1aef 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp @@ -17,6 +17,7 @@ #include #include +#include #include namespace autoware::raw_vehicle_cmd_converter diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp index 0d716eef3369b..698e1f8562ee7 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp @@ -21,6 +21,7 @@ #include "gtest/gtest.h" #include +#include /* * Throttle data: (vel, throttle -> acc) diff --git a/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml b/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml index 919858950e69c..ef298696692bd 100644 --- a/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml +++ b/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/vehicle/autoware_steer_offset_estimator/package.xml b/vehicle/autoware_steer_offset_estimator/package.xml index 561cb85e0d806..27096dd20ab28 100644 --- a/vehicle/autoware_steer_offset_estimator/package.xml +++ b/vehicle/autoware_steer_offset_estimator/package.xml @@ -18,8 +18,8 @@ rclcpp_components std_msgs tier4_debug_msgs + autoware_global_parameter_loader autoware_pose2twist - global_parameter_loader ament_lint_auto autoware_lint_common