diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 205f6c7fb4438..fc9c65dea28a4 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -3,6 +3,7 @@ common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.j common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_geography_utils/** koji.minoda@tier4.jp common/autoware_grid_map_utils/** maxime.clement@tier4.jp +common/autoware_interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/autoware_kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai @@ -23,7 +24,6 @@ common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabay common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp -common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp @@ -84,6 +84,8 @@ localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/* localization/autoware_landmark_based_localizer/autoware_landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/** shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/autoware_localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_pose_covariance_modifier/** melike@leodrive.ai localization/autoware_pose_estimator_arbiter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -91,8 +93,6 @@ localization/autoware_pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tie localization/autoware_pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/autoware_ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -106,7 +106,7 @@ perception/autoware_bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp perception/autoware_cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/autoware_compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp -perception/autoware_detected_object_feature_remover/** tomoya.kimura@tier4.jp yoshi.ri@tier4.jp kotaro.uetake@tier4.jp +perception/autoware_detected_object_feature_remover/** kotaro.uetake@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp perception/autoware_detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp @@ -162,9 +162,9 @@ planning/autoware_scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/autoware_surround_obstacle_checker/** go.sakayori@tier4.jp satoshi.ota@tier4.jp planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp -planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp -planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_planner/autoware_behavior_path_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/autoware_behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp @@ -191,9 +191,9 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_li planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** alqudah.mohammad@tier4.jp maxime.clement@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** alqudah.mohammad@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** alqudah.mohammad@tier4.jp maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** alqudah.mohammad@tier4.jp maxime.clement@tier4.jp planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp diff --git a/README.md b/README.md index 35818990cee95..9636c801a4444 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,5 @@ # Autoware Universe -[![codecov](https://codecov.io/github/autowarefoundation/autoware.universe/graph/badge.svg?token=KQP68YQ65D)](https://codecov.io/github/autowarefoundation/autoware.universe) - ## Welcome to Autoware Universe Autoware Universe serves as a foundational pillar within the Autoware ecosystem, playing a critical role in enhancing the core functionalities of autonomous driving technologies. @@ -16,3 +14,37 @@ To dive into the vast world of Autoware and understand how Autoware Universe fit ### Explore Autoware Universe documentation For those looking to explore the specifics of Autoware Universe components, the [Autoware Universe Documentation](https://autowarefoundation.github.io/autoware.universe/), deployed with MKDocs, offers detailed insights. + +## Code Coverage Metrics + +Below table shows the coverage rate of entire Autoware Universe and sub-components respectively. + +### Entire Project Coverage + +[![codecov](https://codecov.io/github/autowarefoundation/autoware.universe/graph/badge.svg?token=KQP68YQ65D)](https://codecov.io/github/autowarefoundation/autoware.universe) + +### Component-wise Coverage + +You can check more details by clicking the badge and navigating the codecov website. + +| Component | Coverage | +| ------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| Common | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Common%20Packages&query=$.[0].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Common%20Packages) | +| Control | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Control%20Packages&query=$.[1].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Control%20Packages) | +| Evaluator | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Evaluator%20Packages&query=$.[2].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Evaluator%20Packages) | +| Launch | TBD | +| Localization | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Localization%20Packages&query=$.[4].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Localization%20Packages) | +| Map | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Map%20Packages&query=$.[5].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Map%20Packages) | +| Perception | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Perception%20Packages&query=$.[6].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Perception%20Packages) | +| Planning | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Planning%20Packages&query=$.[7].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Planning%20Packages) | +| Sensing | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Sensing%20Packages&query=$.[8].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Sensing%20Packages) | +| Simulator | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Simulator%20Packages&query=$.[9].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Simulator%20Packages) | +| System | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=System%20Packages&query=$.[10].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=System%20Packages) | +| Vehicle | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Vehicle%20Packages&query=$.[11].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Vehicle%20Packages) | + + + diff --git a/codecov.yaml b/codecov.yaml index 75fc6f8bacadd..4c989445a9f54 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -31,9 +31,162 @@ ignore: - "**/test/*" - "**/test/**/*" - "**/debug.*" + - tools/** component_management: individual_components: + # each entire component + - component_id: common-packages + name: Common Packages + paths: + - common/**/** + + - component_id: control-packages + name: Control Packages + paths: + - control/**/** + + - component_id: evaluator-packages + name: Evaluator Packages + paths: + - evaluator/**/** + + - component_id: launch-packages + name: Launch Packages + paths: + - launch/**/** + + - component_id: localization-packages + name: Localization Packages + paths: + - localization/**/** + + - component_id: map-packages + name: map Packages + paths: + - map/**/** + + - component_id: perception-packages + name: Perception Packages + paths: + - perception/**/** + + - component_id: planning-packages + name: Planning Packages + paths: + - planning/**/** + + - component_id: sensing-packages + name: Sensing Packages + paths: + - sensing/**/** + + - component_id: simulator-packages + name: Simulator Packages + paths: + - simulator/**/** + + - component_id: system-packages + name: System Packages + paths: + - system/**/** + + - component_id: vehicle-packages + name: Vehicle Packages + paths: + - vehicle/**/** + + # TIER IV maintained packages + - component_id: control-tier-iv-maintained-packages + name: Control TIER IV Maintained Packages + paths: + - control/autoware_autonomous_emergency_braking/** + - control/autoware_control_validator/** + - control/autoware_external_cmd_selector/** + # - control/autoware_joy_controller/** + - control/autoware_lane_departure_checker/** + - control/autoware_mpc_lateral_controller/** + - control/autoware_operation_mode_transition_manager/** + - control/autoware_pid_longitudinal_controller/** + # - control/autoware_pure_pursuit/** + - control/autoware_shift_decider/** + # - control/autoware_smart_mpc_trajectory_follower/** + - control/autoware_trajectory_follower_base/** + - control/autoware_trajectory_follower_node/** + - control/autoware_vehicle_cmd_gate/** + # - control/control_performance_analysis/** + - control/obstacle_collision_checker/** + # - control/predicted_path_checker/** + + - component_id: localization-tier-iv-maintained-packages + name: Localization TIER IV Maintained Packages + paths: + - localization/autoware_ekf_localizer/** + - localization/autoware_gyro_odometer/** + - localization/autoware_localization_error_monitor/** + - localization/autoware_localization_util/** + - localization/autoware_ndt_scan_matcher/** + - localization/autoware_pose_initializer/** + - localization/autoware_pose_instability_detector/** + - localization/autoware_stop_filter/** + - localization/autoware_twist2accel/** + + - component_id: map-tier-iv-maintained-packages + name: Map TIER IV Maintained Packages + paths: + - map/**/** + + - component_id: perception-tier-iv-maintained-packages + name: Perception TIER IV Maintained Packages + paths: + - perception/autoware_bytetrack/** + - perception/autoware_cluster_merger/** + - perception/autoware_compare_map_segmentation/** + - perception/autoware_crosswalk_traffic_light_estimator/** + - perception/autoware_detected_object_feature_remover/** + - perception/autoware_detected_object_validation/** + - perception/autoware_detection_by_tracker/** + - perception/autoware_elevation_map_loader/** + - perception/autoware_euclidean_cluster/** + - perception/autoware_ground_segmentation/** + - perception/autoware_image_projection_based_fusion/** + - perception/autoware_lidar_centerpoint/** + - perception/autoware_lidar_transfusion/** + - perception/autoware_map_based_prediction/** + - perception/autoware_multi_object_tracker/** + - perception/autoware_object_merger/** + - perception/autoware_object_range_splitter/** + - perception/autoware_object_velocity_splitter/** + - perception/autoware_occupancy_grid_map_outlier_filter/** + - perception/autoware_probabilistic_occupancy_grid_map/** + - perception/autoware_radar_crossing_objects_noise_filter/** + - perception/autoware_radar_fusion_to_detected_object/** + - perception/autoware_radar_object_clustering/** + - perception/autoware_radar_object_tracker/** + - perception/autoware_radar_tracks_msgs_converter/** + - perception/autoware_raindrop_cluster_filter/** + - perception/autoware_shape_estimation/** + - perception/autoware_simple_object_merger/** + - perception/autoware_tensorrt_classifier/** + - perception/autoware_tensorrt_yolox/** + - perception/autoware_tracking_object_merger/** + - perception/autoware_traffic_light_arbiter/** + - perception/autoware_traffic_light_classifier/** + - perception/autoware_traffic_light_fine_detector/** + - perception/autoware_traffic_light_map_based_detector/** + - perception/autoware_traffic_light_multi_camera_fusion/** + - perception/autoware_traffic_light_occlusion_predictor/** + - perception/autoware_traffic_light_visualization/** + - perception/image_projection_based_fusion/** + # - perception/lidar_apollo_instance_segmentation/** + - perception/lidar_centerpoint/** + - perception/perception_utils/** + - perception/tensorrt_yolo/** + - perception/tensorrt_yolox/** + - perception/traffic_light_classifier/** + - perception/traffic_light_fine_detector/** + - perception/traffic_light_ssd_fine_detector/** + - component_id: planning-tier-iv-maintained-packages name: Planning TIER IV Maintained Packages paths: @@ -72,13 +225,13 @@ component_management: ##### behavior_velocity_planner ##### - planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/** - # - planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/** # - planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/** # - planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** - # - planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** - planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** # - planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** @@ -92,24 +245,3 @@ component_management: - planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** - planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** - planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** - - - component_id: control-tier-iv-maintained-packages - name: Control TIER IV Maintained Packages - paths: - - control/autoware_autonomous_emergency_braking/** - - control/autoware_control_validator/** - - control/autoware_external_cmd_selector/** - # - control/autoware_joy_controller/** - - control/autoware_lane_departure_checker/** - - control/autoware_mpc_lateral_controller/** - - control/autoware_operation_mode_transition_manager/** - - control/autoware_pid_longitudinal_controller/** - # - control/autoware_pure_pursuit/** - - control/autoware_shift_decider/** - # - control/autoware_smart_mpc_trajectory_follower/** - - control/autoware_trajectory_follower_base/** - - control/autoware_trajectory_follower_node/** - - control/autoware_vehicle_cmd_gate/** - # - control/control_performance_analysis/** - - control/obstacle_collision_checker/** - # - control/predicted_path_checker/** diff --git a/common/tensorrt_common/package.xml b/common/tensorrt_common/package.xml index f5a3896b55881..f124dc4c29dc6 100644 --- a/common/tensorrt_common/package.xml +++ b/common/tensorrt_common/package.xml @@ -8,6 +8,8 @@ Daisuke Nishimatsu Dan Umeda Manato Hirabayashi + Amadeusz Szymko + Kenzo Lobos-Tsunekawa Apache License 2.0 diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index 25f890799465d..1fcdacef17697 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -60,7 +61,10 @@ std::unique_ptr gradation( } std::unique_ptr setColorDependsOnVelocity( - const double vel_max, const double cmd_vel) + const double vel_max, const double cmd_vel, + const rviz_common::properties::ColorProperty & color_min, + const rviz_common::properties::ColorProperty & color_mid, + const rviz_common::properties::ColorProperty & color_max) { const double cmd_vel_abs = std::fabs(cmd_vel); const double vel_min = 0.0; @@ -68,16 +72,17 @@ std::unique_ptr setColorDependsOnVelocity( std::unique_ptr color_ptr(new Ogre::ColourValue()); if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); - color_ptr = gradation(Qt::red, Qt::yellow, ratio); + color_ptr = gradation(color_min.getColor(), color_mid.getColor(), ratio); } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); - color_ptr = gradation(Qt::yellow, Qt::green, ratio); + color_ptr = gradation(color_mid.getColor(), color_max.getColor(), ratio); } else if (vel_max < cmd_vel_abs) { - *color_ptr = Ogre::ColourValue::Green; + // Use max color when velocity exceeds max + *color_ptr = rviz_common::properties::qtToOgre(color_max.getColor()); } else { - *color_ptr = Ogre::ColourValue::Red; + // Use min color when velocity is below min + *color_ptr = rviz_common::properties::qtToOgre(color_min.getColor()); } - return color_ptr; } @@ -109,8 +114,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay property_path_width_view_{"Constant Width", false, "", &property_path_view_}, property_path_width_{"Width", 2.0, "", &property_path_view_}, property_path_alpha_{"Alpha", 1.0, "", &property_path_view_}, - property_path_color_view_{"Constant Color", false, "", &property_path_view_}, - property_path_color_{"Color", Qt::black, "", &property_path_view_}, + property_min_color_("Min Velocity Color", QColor("#3F2EE3"), "", &property_path_view_), + property_mid_color_("Mid Velocity Color", QColor("#208AAE"), "", &property_path_view_), + property_max_color_("Max Velocity Color", QColor("#00E678"), "", &property_path_view_), + property_fade_out_distance_{"Fade Out Distance", 0.0, "[m]", &property_path_view_}, property_vel_max_{"Color Border Vel Max", 3.0, "[m/s]", this}, // velocity property_velocity_view_{"View Velocity", true, "", this}, @@ -356,6 +363,34 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay const float right = property_path_width_view_.getBool() ? property_path_width_.getFloat() / 2.0 : info->width / 2.0; + // Initialize alphas with the default alpha value + std::vector alphas(msg_ptr->points.size(), property_path_alpha_.getFloat()); + + // Backward iteration to adjust alpha values for the last x meters + if (property_fade_out_distance_.getFloat() > std::numeric_limits::epsilon()) { + alphas.back() = 0.0f; + float cumulative_distance = 0.0f; + for (size_t point_idx = msg_ptr->points.size() - 1; point_idx > 0; point_idx--) { + const auto & curr_point = autoware::universe_utils::getPose(msg_ptr->points.at(point_idx)); + const auto & prev_point = + autoware::universe_utils::getPose(msg_ptr->points.at(point_idx - 1)); + float distance = std::sqrt(autoware::universe_utils::calcSquaredDistance2d( + prev_point.position, curr_point.position)); + cumulative_distance += distance; + + if (cumulative_distance <= property_fade_out_distance_.getFloat()) { + auto ratio = + static_cast(cumulative_distance / property_fade_out_distance_.getFloat()); + float alpha = property_path_alpha_.getFloat() * ratio; + alphas.at(point_idx - 1) = alpha; + } else { + // If the distance exceeds the fade out distance, break the loop + break; + } + } + } + + // Forward iteration to visualize path for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & path_point = msg_ptr->points.at(point_idx); const auto & pose = autoware::universe_utils::getPose(path_point); @@ -364,15 +399,14 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay // path if (property_path_view_.getBool()) { Ogre::ColourValue color; - if (property_path_color_view_.getBool()) { - color = rviz_common::properties::qtToOgre(property_path_color_.getColor()); - } else { - // color change depending on velocity - std::unique_ptr dynamic_color_ptr = - setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); - color = *dynamic_color_ptr; - } - color.a = property_path_alpha_.getFloat(); + + // color change depending on velocity + std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( + property_vel_max_.getFloat(), velocity, property_min_color_, property_mid_color_, + property_max_color_); + color = *dynamic_color_ptr; + color.a = alphas.at(point_idx); + Eigen::Vector3f vec_in; Eigen::Vector3f vec_out; Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); @@ -413,8 +447,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay color = rviz_common::properties::qtToOgre(property_velocity_color_.getColor()); } else { /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = - setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); + std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( + property_vel_max_.getFloat(), velocity, property_min_color_, property_mid_color_, + property_max_color_); color = *dynamic_color_ptr; } color.a = property_velocity_alpha_.getFloat(); @@ -616,8 +651,13 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay rviz_common::properties::BoolProperty property_path_width_view_; rviz_common::properties::FloatProperty property_path_width_; rviz_common::properties::FloatProperty property_path_alpha_; - rviz_common::properties::BoolProperty property_path_color_view_; - rviz_common::properties::ColorProperty property_path_color_; + // Gradient points for velocity color + rviz_common::properties::ColorProperty property_min_color_; + rviz_common::properties::ColorProperty property_mid_color_; + rviz_common::properties::ColorProperty property_max_color_; + // Last x meters of the path will fade out to transparent + rviz_common::properties::FloatProperty property_fade_out_distance_; + rviz_common::properties::FloatProperty property_vel_max_; rviz_common::properties::BoolProperty property_velocity_view_; rviz_common::properties::FloatProperty property_velocity_alpha_; diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp index 8cce8b88a0e7f..dd09c8ee34ce8 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. +// Copyright 2020-2024 Tier IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,7 +15,6 @@ #ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ #define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ -#include #include #include @@ -24,8 +23,6 @@ #include #include -#include - #include #include @@ -54,6 +51,8 @@ struct Input geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; + Param param; }; struct Output @@ -65,43 +64,31 @@ struct Output std::vector vehicle_passing_areas; }; -class ObstacleCollisionChecker -{ -public: - explicit ObstacleCollisionChecker(rclcpp::Node & node); - Output update(const Input & input); - - void setParam(const Param & param) { param_ = param; } +Output check_for_collisions(const Input & input); -private: - Param param_; - autoware::vehicle_info_utils::VehicleInfo vehicle_info_; +//! This function assumes the input trajectory is sampled dense enough +autoware_planning_msgs::msg::Trajectory resample_trajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval); - //! This function assumes the input trajectory is sampled dense enough - static autoware_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval); +autoware_planning_msgs::msg::Trajectory cut_trajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double length); - static autoware_planning_msgs::msg::Trajectory cutTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, const double length); +std::vector create_vehicle_footprints( + const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); - static std::vector createVehicleFootprints( - const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); +std::vector create_vehicle_passing_areas( + const std::vector & vehicle_footprints); - static std::vector createVehiclePassingAreas( - const std::vector & vehicle_footprints); +LinearRing2d create_hull_from_footprints(const LinearRing2d & area1, const LinearRing2d & area2); - static LinearRing2d createHullFromFootprints( - const LinearRing2d & area1, const LinearRing2d & area2); +bool will_collide( + const pcl::PointCloud & obstacle_pointcloud, + const std::vector & vehicle_footprints); - static bool willCollide( - const pcl::PointCloud & obstacle_pointcloud, - const std::vector & vehicle_footprints); - - static bool hasCollision( - const pcl::PointCloud & obstacle_pointcloud, - const LinearRing2d & vehicle_footprint); -}; +bool has_collision( + const pcl::PointCloud & obstacle_pointcloud, + const LinearRing2d & vehicle_footprint); } // namespace obstacle_collision_checker #endif // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp index b08ecccd57282..94f08136e064d 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -38,7 +39,7 @@ namespace obstacle_collision_checker { struct NodeParam { - double update_rate; + double update_rate{}; }; class ObstacleCollisionCheckerNode : public rclcpp::Node @@ -64,12 +65,13 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; // Callback - void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); - void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); - void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); + void on_obstacle_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void on_reference_trajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void on_predicted_trajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void on_odom(const nav_msgs::msg::Odometry::SharedPtr msg); // Publisher std::shared_ptr debug_publisher_; @@ -77,33 +79,31 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node // Timer rclcpp::TimerBase::SharedPtr timer_; - void initTimer(double period_s); + void init_timer(double period_s); - bool isDataReady(); - bool isDataTimeout(); - void onTimer(); + bool is_data_ready(); + bool is_data_timeout(); + void on_timer(); // Parameter NodeParam node_param_; - Param param_; // Dynamic Reconfigure OnSetParametersCallbackHandle::SharedPtr set_param_res_; - rcl_interfaces::msg::SetParametersResult paramCallback( + rcl_interfaces::msg::SetParametersResult param_callback( const std::vector & parameters); // Core Input input_; Output output_; - std::unique_ptr obstacle_collision_checker_; // Diagnostic Updater diagnostic_updater::Updater updater_; - void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat); + void check_lane_departure(diagnostic_updater::DiagnosticStatusWrapper & stat); // Visualization - visualization_msgs::msg::MarkerArray createMarkerArray() const; + visualization_msgs::msg::MarkerArray create_marker_array() const; }; } // namespace obstacle_collision_checker diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index 9fb3657b957c7..b5a2859cfbd54 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -20,24 +20,18 @@ #include #include #include +#include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include #include namespace { -pcl::PointCloud getTransformedPointCloud( +pcl::PointCloud get_transformed_point_cloud( const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const geometry_msgs::msg::Transform & transform) { @@ -52,7 +46,7 @@ pcl::PointCloud getTransformedPointCloud( return transformed_pointcloud; } -pcl::PointCloud filterPointCloudByTrajectory( +pcl::PointCloud filter_point_cloud_by_trajectory( const pcl::PointCloud & pointcloud, const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius) { @@ -70,7 +64,7 @@ pcl::PointCloud filterPointCloudByTrajectory( return filtered_pointcloud; } -double calcBrakingDistance( +double calc_braking_distance( const double abs_velocity, const double max_deceleration, const double delay_time) { const double idling_distance = abs_velocity * delay_time; @@ -82,12 +76,7 @@ double calcBrakingDistance( namespace obstacle_collision_checker { -ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) -: vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) -{ -} - -Output ObstacleCollisionChecker::update(const Input & input) +Output check_for_collisions(const Input & input) { Output output; autoware::universe_utils::StopWatch stop_watch; @@ -97,31 +86,32 @@ Output ObstacleCollisionChecker::update(const Input & input) const auto & raw_abs_velocity = std::abs(input.current_twist->linear.x); const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity; const auto braking_distance = - calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time); - output.resampled_trajectory = cutTrajectory( - resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance); + calc_braking_distance(abs_velocity, input.param.max_deceleration, input.param.delay_time); + output.resampled_trajectory = cut_trajectory( + resample_trajectory(*input.predicted_trajectory, input.param.resample_interval), + braking_distance); output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true); // resample pointcloud const auto obstacle_pointcloud = - getTransformedPointCloud(*input.obstacle_pointcloud, input.obstacle_transform->transform); - const auto filtered_obstacle_pointcloud = filterPointCloudByTrajectory( - obstacle_pointcloud, output.resampled_trajectory, param_.search_radius); + get_transformed_point_cloud(*input.obstacle_pointcloud, input.obstacle_transform->transform); + const auto filtered_obstacle_pointcloud = filter_point_cloud_by_trajectory( + obstacle_pointcloud, output.resampled_trajectory, input.param.search_radius); output.vehicle_footprints = - createVehicleFootprints(output.resampled_trajectory, param_, vehicle_info_); + create_vehicle_footprints(output.resampled_trajectory, input.param, input.vehicle_info); output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true); - output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints); + output.vehicle_passing_areas = create_vehicle_passing_areas(output.vehicle_footprints); output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true); - output.will_collide = willCollide(filtered_obstacle_pointcloud, output.vehicle_passing_areas); + output.will_collide = will_collide(filtered_obstacle_pointcloud, output.vehicle_passing_areas); output.processing_time_map["willCollide"] = stop_watch.toc(true); return output; } -autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( +autoware_planning_msgs::msg::Trajectory resample_trajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval) { autoware_planning_msgs::msg::Trajectory resampled; @@ -131,11 +121,9 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = - autoware::universe_utils::fromMsg(resampled.points.back().pose.position).to_2d(); - const auto p2 = autoware::universe_utils::fromMsg(point.pose.position).to_2d(); - - if (boost::geometry::distance(p1, p2) > interval) { + const auto distance = + autoware::universe_utils::calcDistance2d(resampled.points.back(), point.pose.position); + if (distance > interval) { resampled.points.push_back(point); } } @@ -144,7 +132,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec return resampled; } -autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( +autoware_planning_msgs::msg::Trajectory cut_trajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const double length) { autoware_planning_msgs::msg::Trajectory cut; @@ -157,8 +145,8 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( const auto p1 = autoware::universe_utils::fromMsg(cut.points.back().pose.position); const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); - const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); + const auto points_distance = boost::geometry::distance(p1, p2); const auto remain_distance = length - total_length; // Over length @@ -187,7 +175,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( return cut; } -std::vector ObstacleCollisionChecker::createVehicleFootprints( +std::vector create_vehicle_footprints( const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { @@ -205,7 +193,7 @@ std::vector ObstacleCollisionChecker::createVehicleFootprints( return vehicle_footprints; } -std::vector ObstacleCollisionChecker::createVehiclePassingAreas( +std::vector create_vehicle_passing_areas( const std::vector & vehicle_footprints) { // Create hull from two adjacent vehicle footprints @@ -213,14 +201,13 @@ std::vector ObstacleCollisionChecker::createVehiclePassingAreas( for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) { const auto & footprint1 = vehicle_footprints.at(i); const auto & footprint2 = vehicle_footprints.at(i + 1); - areas.push_back(createHullFromFootprints(footprint1, footprint2)); + areas.push_back(create_hull_from_footprints(footprint1, footprint2)); } return areas; } -LinearRing2d ObstacleCollisionChecker::createHullFromFootprints( - const LinearRing2d & area1, const LinearRing2d & area2) +LinearRing2d create_hull_from_footprints(const LinearRing2d & area1, const LinearRing2d & area2) { autoware::universe_utils::MultiPoint2d combined; for (const auto & p : area1) { @@ -234,16 +221,15 @@ LinearRing2d ObstacleCollisionChecker::createHullFromFootprints( return hull; } -bool ObstacleCollisionChecker::willCollide( +bool will_collide( const pcl::PointCloud & obstacle_pointcloud, const std::vector & vehicle_footprints) { for (size_t i = 1; i < vehicle_footprints.size(); i++) { // skip first footprint because surround obstacle checker handle it const auto & vehicle_footprint = vehicle_footprints.at(i); - if (hasCollision(obstacle_pointcloud, vehicle_footprint)) { - RCLCPP_WARN( - rclcpp::get_logger("obstacle_collision_checker"), "ObstacleCollisionChecker::willCollide"); + if (has_collision(obstacle_pointcloud, vehicle_footprint)) { + RCLCPP_WARN(rclcpp::get_logger("obstacle_collision_checker"), "willCollide"); return true; } } @@ -251,7 +237,7 @@ bool ObstacleCollisionChecker::willCollide( return false; } -bool ObstacleCollisionChecker::hasCollision( +bool has_collision( const pcl::PointCloud & obstacle_pointcloud, const LinearRing2d & vehicle_footprint) { @@ -259,8 +245,8 @@ bool ObstacleCollisionChecker::hasCollision( if (boost::geometry::within( autoware::universe_utils::Point2d{point.x, point.y}, vehicle_footprint)) { RCLCPP_WARN( - rclcpp::get_logger("obstacle_collision_checker"), - "[ObstacleCollisionChecker] Collide to Point x: %f y: %f", point.x, point.y); + rclcpp::get_logger("obstacle_collision_checker"), "Collide to Point x: %f y: %f", point.x, + point.y); return true; } } diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index 9af5fe24b17ea..0b0ac42589a72 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. +// Copyright 2020-2024 Tier IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,37 +17,19 @@ #include #include #include +#include #include #include #include -#include #include -namespace -{ -template -bool update_param( - const std::vector & params, const std::string & name, T & value) -{ - const auto itr = std::find_if( - params.cbegin(), params.cend(), - [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); - - // Not found - if (itr == params.cend()) { - return false; - } - - value = itr->template get_value(); - return true; -} -} // namespace - namespace obstacle_collision_checker { ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOptions & node_options) -: Node("obstacle_collision_checker_node", node_options), updater_(this) +: Node("obstacle_collision_checker_node", node_options), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), + updater_(this) { using std::placeholders::_1; @@ -55,19 +37,15 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt node_param_.update_rate = declare_parameter("update_rate"); // Core Parameter - param_.delay_time = declare_parameter("delay_time"); - param_.footprint_margin = declare_parameter("footprint_margin"); - param_.max_deceleration = declare_parameter("max_deceleration"); - param_.resample_interval = declare_parameter("resample_interval"); - param_.search_radius = declare_parameter("search_radius"); + input_.param.delay_time = declare_parameter("delay_time"); + input_.param.footprint_margin = declare_parameter("footprint_margin"); + input_.param.max_deceleration = declare_parameter("max_deceleration"); + input_.param.resample_interval = declare_parameter("resample_interval"); + input_.param.search_radius = declare_parameter("search_radius"); // Dynamic Reconfigure - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ObstacleCollisionCheckerNode::paramCallback, this, _1)); - - // Core - obstacle_collision_checker_ = std::make_unique(*this); - obstacle_collision_checker_->setParam(param_); + set_param_res_ = this->add_on_set_parameters_callback(std::bind( + &::obstacle_collision_checker::ObstacleCollisionCheckerNode::param_callback, this, _1)); // Subscriber self_pose_listener_ = std::make_shared(this); @@ -75,15 +53,15 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt sub_obstacle_pointcloud_ = create_subscription( "input/obstacle_pointcloud", rclcpp::SensorDataQoS(), - std::bind(&ObstacleCollisionCheckerNode::onObstaclePointcloud, this, _1)); + std::bind(&ObstacleCollisionCheckerNode::on_obstacle_pointcloud, this, _1)); sub_reference_trajectory_ = create_subscription( "input/reference_trajectory", 1, - std::bind(&ObstacleCollisionCheckerNode::onReferenceTrajectory, this, _1)); + std::bind(&ObstacleCollisionCheckerNode::on_reference_trajectory, this, _1)); sub_predicted_trajectory_ = create_subscription( "input/predicted_trajectory", 1, - std::bind(&ObstacleCollisionCheckerNode::onPredictedTrajectory, this, _1)); + std::bind(&ObstacleCollisionCheckerNode::on_predicted_trajectory, this, _1)); sub_odom_ = create_subscription( - "input/odometry", 1, std::bind(&ObstacleCollisionCheckerNode::onOdom, this, _1)); + "input/odometry", 1, std::bind(&ObstacleCollisionCheckerNode::on_odom, this, _1)); // Publisher debug_publisher_ = @@ -94,47 +72,47 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt updater_.setHardwareID("obstacle_collision_checker"); updater_.add( - "obstacle_collision_checker", this, &ObstacleCollisionCheckerNode::checkLaneDeparture); + "obstacle_collision_checker", this, &ObstacleCollisionCheckerNode::check_lane_departure); // Wait for first self pose self_pose_listener_->waitForFirstPose(); // Timer - initTimer(1.0 / node_param_.update_rate); + init_timer(1.0 / node_param_.update_rate); } -void ObstacleCollisionCheckerNode::onObstaclePointcloud( +void ObstacleCollisionCheckerNode::on_obstacle_pointcloud( const sensor_msgs::msg::PointCloud2::SharedPtr msg) { obstacle_pointcloud_ = msg; } -void ObstacleCollisionCheckerNode::onReferenceTrajectory( +void ObstacleCollisionCheckerNode::on_reference_trajectory( const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { reference_trajectory_ = msg; } -void ObstacleCollisionCheckerNode::onPredictedTrajectory( +void ObstacleCollisionCheckerNode::on_predicted_trajectory( const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { predicted_trajectory_ = msg; } -void ObstacleCollisionCheckerNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +void ObstacleCollisionCheckerNode::on_odom(const nav_msgs::msg::Odometry::SharedPtr msg) { current_twist_ = std::make_shared(msg->twist.twist); } -void ObstacleCollisionCheckerNode::initTimer(double period_s) +void ObstacleCollisionCheckerNode::init_timer(double period_s) { const auto period_ns = std::chrono::duration_cast(std::chrono::duration(period_s)); timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&ObstacleCollisionCheckerNode::onTimer, this)); + this, get_clock(), period_ns, std::bind(&ObstacleCollisionCheckerNode::on_timer, this)); } -bool ObstacleCollisionCheckerNode::isDataReady() +bool ObstacleCollisionCheckerNode::is_data_ready() { if (!current_pose_) { RCLCPP_INFO_THROTTLE( @@ -178,7 +156,7 @@ bool ObstacleCollisionCheckerNode::isDataReady() return true; } -bool ObstacleCollisionCheckerNode::isDataTimeout() +bool ObstacleCollisionCheckerNode::is_data_timeout() { const auto now = this->now(); @@ -193,7 +171,7 @@ bool ObstacleCollisionCheckerNode::isDataTimeout() return false; } -void ObstacleCollisionCheckerNode::onTimer() +void ObstacleCollisionCheckerNode::on_timer() { current_pose_ = self_pose_listener_->getCurrentPose(); if (obstacle_pointcloud_) { @@ -209,11 +187,11 @@ void ObstacleCollisionCheckerNode::onTimer() } } - if (!isDataReady()) { + if (!is_data_ready()) { return; } - if (isDataTimeout()) { + if (is_data_timeout()) { return; } @@ -223,17 +201,18 @@ void ObstacleCollisionCheckerNode::onTimer() input_.reference_trajectory = reference_trajectory_; input_.predicted_trajectory = predicted_trajectory_; input_.current_twist = current_twist_; + input_.vehicle_info = vehicle_info_; - output_ = obstacle_collision_checker_->update(input_); + output_ = check_for_collisions(input_); updater_.force_update(); - debug_publisher_->publish("marker_array", createMarkerArray()); + debug_publisher_->publish("marker_array", create_marker_array()); time_publisher_->publish(output_.processing_time_map); } -rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::paramCallback( +rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::param_callback( const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; @@ -241,25 +220,22 @@ rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::paramCall result.reason = "success"; try { + using autoware::universe_utils::updateParam; // Node Parameter { auto & p = node_param_; // Update params - update_param(parameters, "update_rate", p.update_rate); + updateParam(parameters, "update_rate", p.update_rate); } - auto & p = param_; - - update_param(parameters, "delay_time", p.delay_time); - update_param(parameters, "footprint_margin", p.footprint_margin); - update_param(parameters, "max_deceleration", p.max_deceleration); - update_param(parameters, "resample_interval", p.resample_interval); - update_param(parameters, "search_radius", p.search_radius); + auto & p = input_.param; - if (obstacle_collision_checker_) { - obstacle_collision_checker_->setParam(param_); - } + updateParam(parameters, "delay_time", p.delay_time); + updateParam(parameters, "footprint_margin", p.footprint_margin); + updateParam(parameters, "max_deceleration", p.max_deceleration); + updateParam(parameters, "resample_interval", p.resample_interval); + updateParam(parameters, "search_radius", p.search_radius); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); @@ -267,7 +243,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::paramCall return result; } -void ObstacleCollisionCheckerNode::checkLaneDeparture( +void ObstacleCollisionCheckerNode::check_lane_departure( diagnostic_updater::DiagnosticStatusWrapper & stat) { int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -281,7 +257,7 @@ void ObstacleCollisionCheckerNode::checkLaneDeparture( stat.summary(level, msg); } -visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::createMarkerArray() const +visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::create_marker_array() const { using autoware::universe_utils::createDefaultMarker; using autoware::universe_utils::createMarkerColor; @@ -339,8 +315,8 @@ visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::createMarkerA for (const auto & vehicle_footprint : output_.vehicle_footprints) { for (size_t i = 0; i < vehicle_footprint.size() - 1; ++i) { - const auto p1 = vehicle_footprint.at(i); - const auto p2 = vehicle_footprint.at(i + 1); + const auto & p1 = vehicle_footprint.at(i); + const auto & p2 = vehicle_footprint.at(i + 1); marker.points.push_back(toMsg(p1.to_3d(base_link_z))); marker.points.push_back(toMsg(p2.to_3d(base_link_z))); diff --git a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp index 705bff754d3d9..581b75ca463ab 100644 --- a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp @@ -14,6 +14,53 @@ #include "../src/obstacle_collision_checker_node/obstacle_collision_checker.cpp" // NOLINT #include "gtest/gtest.h" +#include "obstacle_collision_checker/obstacle_collision_checker.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace +{ +pcl::PointXYZ pcl_point(const float x, const float y) +{ + pcl::PointXYZ p; + p.x = x; + p.y = y; + p.z = 0.0; + return p; +} + +pcl::PointCloud pcl_pointcloud(const std::vector> & points) +{ + pcl::PointCloud pcl; + for (const auto & p : points) { + pcl.push_back(pcl_point(p.first, p.second)); + } + return pcl; +} + +bool point_in_pcl_pointcloud(const pcl::PointXYZ & pt, const pcl::PointCloud & pcd) +{ + for (const auto & p : pcd) { + if (p.x == pt.x && p.y == pt.y && p.z == pt.z) { + return true; + } + } + return false; +} +} // namespace TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) { @@ -23,7 +70,7 @@ TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) autoware_planning_msgs::msg::TrajectoryPoint traj_point; pcl_point.y = 0.0; traj_point.pose.position.y = 0.99; - for (double x = 0.0; x < 10.0; x += 1.0) { + for (float x = 0.0; x < 10.0; x += 1.0) { pcl_point.x = x; traj_point.pose.position.x = x; trajectory.points.push_back(traj_point); @@ -31,12 +78,12 @@ TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) } // radius < 1: all points are filtered for (auto radius = 0.0; radius <= 0.99; radius += 0.1) { - const auto filtered_pcl = filterPointCloudByTrajectory(pcl, trajectory, radius); + const auto filtered_pcl = filter_point_cloud_by_trajectory(pcl, trajectory, radius); EXPECT_EQ(filtered_pcl.size(), 0ul); } // radius >= 1.0: all points are kept for (auto radius = 1.0; radius < 10.0; radius += 0.1) { - const auto filtered_pcl = filterPointCloudByTrajectory(pcl, trajectory, radius); + const auto filtered_pcl = filter_point_cloud_by_trajectory(pcl, trajectory, radius); ASSERT_EQ(pcl.size(), filtered_pcl.size()); for (size_t i = 0; i < pcl.size(); ++i) { EXPECT_EQ(pcl[i].x, filtered_pcl[i].x); @@ -44,3 +91,165 @@ TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) } } } + +TEST(test_obstacle_collision_checker, getTransformedPointCloud) +{ + sensor_msgs::msg::PointCloud2 pcd_msg; + const auto pcl_pcd = pcl_pointcloud({ + {0.0, 0.0}, + {1.0, 1.0}, + {-2.0, 3.0}, + }); + pcl::toROSMsg(pcl_pcd, pcd_msg); + + { // empty transform, expect same points + geometry_msgs::msg::Transform transform; + const auto transformed_pcd = get_transformed_point_cloud(pcd_msg, transform); + EXPECT_EQ(pcl_pcd.size(), transformed_pcd.size()); + for (const auto & p : transformed_pcd.points) { + EXPECT_TRUE(point_in_pcl_pointcloud(p, pcl_pcd)); + } + } + + { // translation + geometry_msgs::msg::Transform transform; + transform.translation.x = 2.0; + transform.translation.y = 1.5; + const auto transformed_pcd = get_transformed_point_cloud(pcd_msg, transform); + EXPECT_EQ(pcl_pcd.size(), transformed_pcd.size()); + for (const auto & p : transformed_pcd.points) { + auto transformed_p = p; + transformed_p.x -= static_cast(transform.translation.x); + transformed_p.y -= static_cast(transform.translation.y); + EXPECT_TRUE(point_in_pcl_pointcloud(transformed_p, pcl_pcd)); + } + } + { // rotation + geometry_msgs::msg::Transform transform; + transform.rotation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, M_PI); + const auto transformed_pcd = get_transformed_point_cloud(pcd_msg, transform); + EXPECT_EQ(pcl_pcd.size(), transformed_pcd.size()); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(0.0, 0.0), transformed_pcd)); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(-1.0, -1.0), transformed_pcd)); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(2.0, -3.0), transformed_pcd)); + } + { // translation + rotation + geometry_msgs::msg::Transform transform; + transform.translation.x = 0.5; + transform.translation.y = -0.5; + transform.rotation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, M_PI); + const auto transformed_pcd = get_transformed_point_cloud(pcd_msg, transform); + EXPECT_EQ(pcl_pcd.size(), transformed_pcd.size()); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(0.5, -0.5), transformed_pcd)); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(-0.5, -1.5), transformed_pcd)); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(2.5, -3.5), transformed_pcd)); + } +} + +TEST(test_obstacle_collision_checker, calcBrakingDistance) +{ + EXPECT_TRUE(std::isnan(calc_braking_distance(0.0, 0.0, 0.0))); + // if we cannot decelerate (max_decel = 0.0), then the result is infinity + EXPECT_DOUBLE_EQ(calc_braking_distance(1.0, 0.0, 1.0), std::numeric_limits::infinity()); + EXPECT_DOUBLE_EQ( + calc_braking_distance(1.0, 1.0, 1.0), + // 1s * 1m/s = 1m for the delay, then 1->0m/s at 1m/s² = 0.5m + 1.0 + 0.5); +} + +TEST(test_obstacle_collision_checker, check_for_collisions) +{ + obstacle_collision_checker::Input input; + // call with empty input causes a segfault + // const auto output = check_for_collisions(input); + input.param.delay_time = 1.0; + input.param.footprint_margin = 0.0; + input.param.max_deceleration = 1.0; + input.param.resample_interval = 1.0; + input.param.search_radius = 10.0; + geometry_msgs::msg::PoseStamped ego_pose; // default (0,0) ego pose + geometry_msgs::msg::TransformStamped tf; // (0,0) transform + tf.transform.rotation.w = 1.0; + input.current_pose = std::make_shared(ego_pose); + input.obstacle_transform = std::make_shared(tf); + // 2mx2m footprint + input.vehicle_info.front_overhang_m = 1.0; + input.vehicle_info.wheel_base_m = 0.0; + input.vehicle_info.rear_overhang_m = 1.0; + input.vehicle_info.left_overhang_m = 1.0; + input.vehicle_info.right_overhang_m = 1.0; + autoware_planning_msgs::msg::Trajectory trajectory; + autoware_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.y = 0.0; + for (auto x = 0.0; x < 6.0; x += 0.1) { + point.pose.position.x = x; + trajectory.points.push_back(point); + } + input.predicted_trajectory = + std::make_shared(trajectory); + input.reference_trajectory = {}; // TODO(someone): the reference trajectory is not used + { + // obstacle point on the trajectory + sensor_msgs::msg::PointCloud2 pcd_msg; + const auto pcl_pcd = pcl_pointcloud({ + {5.0, 0.0}, + }); + pcl::toROSMsg(pcl_pcd, pcd_msg); + input.obstacle_pointcloud = std::make_shared(pcd_msg); + geometry_msgs::msg::Twist twist; // no velocity -> no collision + twist.linear.x = 0.0; + input.current_twist = std::make_shared(twist); + const auto output = check_for_collisions(input); + EXPECT_FALSE(output.will_collide); + // zero velocity: only the 1st point of the trajectory is kept + EXPECT_EQ(output.resampled_trajectory.points.size(), 1UL); + } + { + // moderate velocity: short braking distance so the trajectory is cut before the collision + geometry_msgs::msg::Twist twist; + twist.linear.x = 1.0; + input.current_twist = std::make_shared(twist); + const auto output = check_for_collisions(input); + EXPECT_FALSE(output.will_collide); + // 1s * 1m/s = 1m for the delay, then 1->0m/s at 1m/s² = 0.5m -> 1.5m braking distance + EXPECT_DOUBLE_EQ( + autoware::universe_utils::calcDistance2d( + output.resampled_trajectory.points.front(), output.resampled_trajectory.points.back()), + 1.5); + } + { + // high velocity -> collision + geometry_msgs::msg::Twist twist; + twist.linear.x = 10.0; + input.current_twist = std::make_shared(twist); + const auto output = check_for_collisions(input); + EXPECT_TRUE(output.will_collide); + // high velocity: the full trajectory is resampled (original interval = 0.1, resample interval + // = 1.0) + EXPECT_EQ( + output.resampled_trajectory.points.size(), + input.predicted_trajectory->points.size() / 10 + 1); + } + { + // obstacle point on the side of the trajectory but inside the ego footprint -> collision + sensor_msgs::msg::PointCloud2 pcd_msg; + const auto pcl_pcd = pcl_pointcloud({ + {5.0, 0.5}, + }); + pcl::toROSMsg(pcl_pcd, pcd_msg); + input.obstacle_pointcloud = std::make_shared(pcd_msg); + const auto output = check_for_collisions(input); + EXPECT_TRUE(output.will_collide); + } + { + // obstacle point on the side of the trajectory and outside the ego footprint -> no collision + sensor_msgs::msg::PointCloud2 pcd_msg; + const auto pcl_pcd = pcl_pointcloud({ + {5.0, 1.5}, + }); + pcl::toROSMsg(pcl_pcd, pcd_msg); + input.obstacle_pointcloud = std::make_shared(pcd_msg); + const auto output = check_for_collisions(input); + EXPECT_FALSE(output.will_collide); + } +} diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py index 4fda3521c07e9..16d48313eb2c9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py @@ -35,15 +35,6 @@ def __init__(self, context): with open(pointcloud_map_filter_param_path, "r") as f: self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] - self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] - self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"] - self.timer_interval_ms = self.pointcloud_map_filter_param["timer_interval_ms"] - self.use_dynamic_map_loading = self.pointcloud_map_filter_param["use_dynamic_map_loading"] - self.map_update_distance_threshold = self.pointcloud_map_filter_param[ - "map_update_distance_threshold" - ] - self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] - self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"] self.use_pointcloud_map = LaunchConfiguration("use_pointcloud_map").perform(context) def create_pipeline(self): @@ -116,16 +107,10 @@ def create_compare_map_pipeline(self): ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ + self.pointcloud_map_filter_param, { - "distance_threshold": self.distance_threshold, - "downsize_ratio_z_axis": self.downsize_ratio_z_axis, - "timer_interval_ms": self.timer_interval_ms, - "use_dynamic_map_loading": self.use_dynamic_map_loading, - "map_update_distance_threshold": self.map_update_distance_threshold, - "map_loader_radius": self.map_loader_radius, - "publish_debug_pcd": self.publish_debug_pcd, "input_frame": "map", - } + }, ], extra_arguments=[ {"use_intra_process_comms": False}, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index b84f8a4bd3bd7..821e470054f04 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -64,6 +64,8 @@ class BicycleMotionModel : public MotionModel double lr_min = 1.0; // [m] minimum distance from the center to the rear wheel double max_vel = 27.8; // [m/s] maximum velocity, 100km/h double max_slip = 0.5236; // [rad] maximum slip angle, 30deg + double max_reverse_vel = + -1.389; // [m/s] maximum reverse velocity, -5km/h. The value is expected to be negative } motion_params_; public: diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 812b91fc8acf0..8165b126eda8e 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -45,13 +45,14 @@ class CTRVMotionModel : public MotionModel // motion parameters: process noise and motion limits struct MotionParams { - double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s - double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s - double q_cov_yaw = 0.1218; // [rad^2/s^2] uncertain yaw angle, 20deg/s - double q_cov_vel = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 - double q_cov_wz = 0.5236; // [rad^2/s^4] uncertain yaw rate, 30deg/s^2 - double max_vel = 2.78; // [m/s] maximum velocity - double max_wz = 0.5236; // [rad/s] maximum yaw rate, 30deg/s + double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s + double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s + double q_cov_yaw = 0.1218; // [rad^2/s^2] uncertain yaw angle, 20deg/s + double q_cov_vel = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double q_cov_wz = 0.5236; // [rad^2/s^4] uncertain yaw rate, 30deg/s^2 + double max_vel = 2.78; // [m/s] maximum velocity + double max_wz = 0.5236; // [rad/s] maximum yaw rate, 30deg/s + double max_reverse_vel = -1.38; // [m/s] maximum reverse velocity, -5km/h } motion_params_; public: diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 3c088b8f64b39..399634b63bffe 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -226,15 +226,26 @@ bool BicycleMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + + // maximum reverse velocity + if (motion_params_.max_reverse_vel < 0 && X_t(IDX::VEL) < motion_params_.max_reverse_vel) { + // rotate the object orientation by 180 degrees + X_t(IDX::VEL) = -X_t(IDX::VEL); + X_t(IDX::YAW) = X_t(IDX::YAW) + M_PI; + } + // maximum velocity if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } + // maximum slip angle if (!(-motion_params_.max_slip <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= motion_params_.max_slip)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -motion_params_.max_slip : motion_params_.max_slip; } - ekf_.init(X_t, P_t); + // normalize yaw + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + // overwrite state + ekf_.init(X_t, P_t); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index a838bf62e5bcb..6f63ecbdce06d 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -203,15 +203,26 @@ bool CTRVMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + + // maximum reverse velocity + if (X_t(IDX::VEL) < 0 && X_t(IDX::VEL) < motion_params_.max_reverse_vel) { + // rotate the object orientation by 180 degrees + X_t(IDX::VEL) = -X_t(IDX::VEL); + X_t(IDX::YAW) = X_t(IDX::YAW) + M_PI; + } + // maximum velocity if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } + // maximum yaw rate if (!(-motion_params_.max_wz <= X_t(IDX::WZ) && X_t(IDX::WZ) <= motion_params_.max_wz)) { X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -motion_params_.max_wz : motion_params_.max_wz; } - ekf_.init(X_t, P_t); + // normalize yaw + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + // overwrite state + ekf_.init(X_t, P_t); return true; } diff --git a/planning/autoware_mission_planner/CMakeLists.txt b/planning/autoware_mission_planner/CMakeLists.txt index 74bc8ddbc0a32..939903c2999ff 100644 --- a/planning/autoware_mission_planner/CMakeLists.txt +++ b/planning/autoware_mission_planner/CMakeLists.txt @@ -38,7 +38,8 @@ pluginlib_export_plugin_description_file(autoware_mission_planner plugins/plugin if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_lanelet2_plugins_default_planner.cpp + test/test_lanelet2_plugins_default_planner.cpp + test/test_utility_functions.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}_lanelet2_plugins diff --git a/planning/autoware_mission_planner/test/test_utility_functions.cpp b/planning/autoware_mission_planner/test/test_utility_functions.cpp new file mode 100644 index 0000000000000..6cde09b7664a2 --- /dev/null +++ b/planning/autoware_mission_planner/test/test_utility_functions.cpp @@ -0,0 +1,423 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include <../src/lanelet2_plugins/utility_functions.hpp> +#include +#include +#include + +#include +#include +#include +#include +#include + +using autoware::mission_planner::lanelet2::convert_linear_ring_to_polygon; +using autoware::mission_planner::lanelet2::convertBasicPoint3dToPose; +using autoware::mission_planner::lanelet2::convertCenterlineToPoints; +using autoware::mission_planner::lanelet2::exists; +using autoware::mission_planner::lanelet2::get_closest_centerline_pose; +using autoware::mission_planner::lanelet2::insert_marker_array; +using autoware::mission_planner::lanelet2::is_in_lane; +using autoware::mission_planner::lanelet2::is_in_parking_lot; +using autoware::mission_planner::lanelet2::is_in_parking_space; +using autoware::mission_planner::lanelet2::project_goal_to_map; + +using autoware::vehicle_info_utils::VehicleInfo; +using geometry_msgs::msg::Pose; + +TEST(TestUtilityFunctions, convertLinearRingToPolygon) +{ + // clockwise + { + autoware::universe_utils::LinearRing2d footprint; + footprint.push_back({1.0, 1.0}); + footprint.push_back({1.0, -1.0}); + footprint.push_back({0.0, -1.0}); + footprint.push_back({-1.0, -1.0}); + footprint.push_back({-1.0, 1.0}); + footprint.push_back({0.0, 1.0}); + footprint.push_back({1.0, 1.0}); + autoware::universe_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); + + ASSERT_EQ(polygon.outer().size(), footprint.size()); + for (std::size_t i = 0; i < footprint.size(); ++i) { + EXPECT_DOUBLE_EQ( + boost::geometry::get<0>(polygon.outer()[i]), boost::geometry::get<0>(footprint[i])); + EXPECT_DOUBLE_EQ( + boost::geometry::get<1>(polygon.outer()[i]), boost::geometry::get<1>(footprint[i])); + } + + EXPECT_EQ(polygon.outer().front(), polygon.outer().back()); + + const double area = boost::geometry::area(polygon); + EXPECT_GT(area, 0.0); + } + + // counterclockwise + { + autoware::universe_utils::LinearRing2d footprint; + footprint.push_back({1.0, 1.0}); + footprint.push_back({0.0, 1.0}); + footprint.push_back({-1.0, 1.0}); + footprint.push_back({-1.0, -1.0}); + footprint.push_back({0.0, -1.0}); + footprint.push_back({1.0, -1.0}); + footprint.push_back({1.0, 1.0}); + autoware::universe_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); + + ASSERT_EQ(polygon.outer().size(), footprint.size()); + + // polygon is always clockwise + for (std::size_t i = 0; i < footprint.size(); ++i) { + const std::size_t j = footprint.size() - i - 1; + EXPECT_DOUBLE_EQ( + boost::geometry::get<0>(polygon.outer()[i]), boost::geometry::get<0>(footprint[j])); + EXPECT_DOUBLE_EQ( + boost::geometry::get<1>(polygon.outer()[i]), boost::geometry::get<1>(footprint[j])); + } + + const double area = boost::geometry::area(polygon); + EXPECT_GT(area, 0.0); + } +} + +TEST(TestUtilityFunctions, convertCenterlineToPoints) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + const std::vector points = convertCenterlineToPoints(lanelet); + + ASSERT_EQ(points.size(), centerline.size()); + for (std::size_t i = 0; i < centerline.size(); ++i) { + EXPECT_DOUBLE_EQ(points[i].x, centerline[i].x()); + EXPECT_DOUBLE_EQ(points[i].y, centerline[i].y()); + EXPECT_DOUBLE_EQ(points[i].z, centerline[i].z()); + } +} + +TEST(TestUtilityFunctions, insertMarkerArray) +{ + visualization_msgs::msg::MarkerArray a1; + visualization_msgs::msg::MarkerArray a2; + a1.markers.resize(1); + a2.markers.resize(2); + a1.markers[0].id = 0; + a2.markers[0].id = 1; + a2.markers[1].id = 2; + + insert_marker_array(&a1, a2); + + ASSERT_EQ(a1.markers.size(), 3); + EXPECT_EQ(a1.markers[0].id, 0); + EXPECT_EQ(a1.markers[1].id, 1); + EXPECT_EQ(a1.markers[2].id, 2); +} + +TEST(TestUtilityFunctions, convertBasicPoint3dToPose) +{ + { + const lanelet::BasicPoint3d point(1.0, 2.0, 3.0); + const double lane_yaw = 0.0; + const Pose pose = convertBasicPoint3dToPose(point, lane_yaw); + EXPECT_DOUBLE_EQ(pose.position.x, point.x()); + EXPECT_DOUBLE_EQ(pose.position.y, point.y()); + EXPECT_DOUBLE_EQ(pose.position.z, point.z()); + EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.w, 1.0); + } + + { + const lanelet::BasicPoint3d point(1.0, 2.0, 3.0); + const double lane_yaw = M_PI_2; + const Pose pose = convertBasicPoint3dToPose(point, lane_yaw); + EXPECT_DOUBLE_EQ(pose.position.x, point.x()); + EXPECT_DOUBLE_EQ(pose.position.y, point.y()); + EXPECT_DOUBLE_EQ(pose.position.z, point.z()); + EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.z, 0.7071067811865476); + EXPECT_DOUBLE_EQ(pose.orientation.w, 0.7071067811865476); + } +} + +TEST(TestUtilityFunctions, is_in_lane) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -1}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } +} + +TEST(TestUtilityFunctions, is_in_parking_lot) +{ + lanelet::Polygon3d parking_lot; + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } +} + +TEST(TestUtilityFunctions, is_in_parking_space) +{ + lanelet::LineString3d parking_space; + parking_space.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + parking_space.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + parking_space.setAttribute("width", 2.0); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -1, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } +} + +TEST(TestUtilityFunctions, project_goal_to_map) +{ + const auto create_lane = [&](const double height) -> lanelet::Lanelet { + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1, height}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1, height}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1, height}); + const lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + return lanelet; + }; + + const auto check_height = [&](const double height) -> void { + const auto lanelet = create_lane(height); + lanelet::Point3d goal_point{lanelet::InvalId, 0, 0, height}; + EXPECT_DOUBLE_EQ(project_goal_to_map(lanelet, goal_point), height); + }; + + check_height(0.0); + check_height(1.0); + check_height(-1.0); +} + +TEST(TestUtilityFunctions, TestUtilityFunctions) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + VehicleInfo vehicle_info; + vehicle_info.left_overhang_m = 0.5; + vehicle_info.right_overhang_m = 0.5; + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -1, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, -1.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, -2.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -1}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } +} diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index fdffc8a926c24..8b48f78d117bb 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -477,8 +477,13 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData const auto get_prev_optimized_traj_points = [&]() { if (prev_optimized_traj_points_ptr_) { + RCLCPP_WARN(logger_, "return the previous optimized_trajectory as exceptional behavior."); return *prev_optimized_traj_points_ptr_; } + RCLCPP_WARN( + logger_, + "Try to return the previous optimized_trajectory as exceptional behavior, " + "but this failure also. Then return path_smoother output."); return traj_points; }; @@ -505,8 +510,7 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData // 6. optimize steer angles const auto optimized_variables = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat); if (!optimized_variables) { - RCLCPP_INFO_EXPRESSION( - logger_, enable_debug_info_, "return std::nullopt since could not solve qp"); + RCLCPP_WARN(logger_, "return std::nullopt since could not solve qp"); return get_prev_optimized_traj_points(); } diff --git a/planning/autoware_surround_obstacle_checker/CMakeLists.txt b/planning/autoware_surround_obstacle_checker/CMakeLists.txt index 1b13843de511c..bd6af24577eac 100644 --- a/planning/autoware_surround_obstacle_checker/CMakeLists.txt +++ b/planning/autoware_surround_obstacle_checker/CMakeLists.txt @@ -8,6 +8,14 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS common) +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-error=deprecated-declarations) +endif() + include_directories( SYSTEM ${EIGEN3_INCLUDE_DIR} @@ -32,6 +40,15 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_node ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_surround_obstacle_checker.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + ament_auto_package( INSTALL_TO_SHARE config diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 8362962f3dadb..f1d0d3e9fb76d 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -18,6 +18,7 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs + autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils diagnostic_msgs @@ -36,8 +37,10 @@ tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index d841e152cd596..719a507ab569b 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -14,6 +14,7 @@ #include "node.hpp" +#include #include #include #include @@ -81,70 +82,6 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( no_start_reason_diag.values.push_back(no_start_reason_diag_kv); return no_start_reason_diag; } - -geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const double z) -{ - geometry_msgs::msg::Point32 p; - p.x = x; - p.y = y; - p.z = z; - return p; -} - -Polygon2d createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint) -{ - geometry_msgs::msg::Polygon transformed_polygon{}; - geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(pose); - tf2::doTransform(footprint, transformed_polygon, geometry_tf); - - Polygon2d object_polygon; - for (const auto & p : transformed_polygon.points) { - object_polygon.outer().push_back(Point2d(p.x, p.y)); - } - - bg::correct(object_polygon); - - return object_polygon; -} - -Polygon2d createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) -{ - const double & length_m = size.x / 2.0; - const double & width_m = size.y / 2.0; - - geometry_msgs::msg::Polygon polygon{}; - - polygon.points.push_back(createPoint32(length_m, -width_m, 0.0)); - polygon.points.push_back(createPoint32(length_m, width_m, 0.0)); - polygon.points.push_back(createPoint32(-length_m, width_m, 0.0)); - polygon.points.push_back(createPoint32(-length_m, -width_m, 0.0)); - - return createObjPolygon(pose, polygon); -} - -Polygon2d createSelfPolygon( - const VehicleInfo & vehicle_info, const double front_margin, const double side_margin, - const double rear_margin) -{ - const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin; - const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin; - const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin; - const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin; - - Polygon2d ego_polygon; - - ego_polygon.outer().push_back(Point2d(front_m, width_left_m)); - ego_polygon.outer().push_back(Point2d(front_m, width_right_m)); - ego_polygon.outer().push_back(Point2d(rear_m, width_right_m)); - ego_polygon.outer().push_back(Point2d(rear_m, width_left_m)); - - bg::correct(ego_polygon); - - return ego_polygon; -} } // namespace SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options) @@ -256,7 +193,11 @@ void SurroundObstacleCheckerNode::onTimer() const auto is_obstacle_found = !nearest_obstacle ? false : nearest_obstacle.value().first < epsilon; - if (!isStopRequired(is_obstacle_found, is_vehicle_stopped)) { + bool is_stop_required = false; + std::tie(is_stop_required, last_obstacle_found_time_) = isStopRequired( + is_obstacle_found, is_vehicle_stopped, state_, last_obstacle_found_time_, + param.state_clear_time); + if (!is_stop_required) { break; } @@ -281,7 +222,11 @@ void SurroundObstacleCheckerNode::onTimer() : nearest_obstacle.value().first < param.surround_check_hysteresis_distance; - if (isStopRequired(is_obstacle_found, is_vehicle_stopped)) { + bool is_stop_required = false; + std::tie(is_stop_required, last_obstacle_found_time_) = isStopRequired( + is_obstacle_found, is_vehicle_stopped, state_, last_obstacle_found_time_, + param.state_clear_time); + if (is_stop_required) { break; } @@ -365,7 +310,11 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCl const double front_margin = pointcloud_param.surround_check_front_distance; const double side_margin = pointcloud_param.surround_check_side_distance; const double back_margin = pointcloud_param.surround_check_back_distance; - const auto ego_polygon = createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); + const double base_to_front = vehicle_info_.max_longitudinal_offset_m + front_margin; + const double base_to_rear = vehicle_info_.rear_overhang_m + back_margin; + const double width = vehicle_info_.vehicle_width_m + side_margin * 2; + const auto ego_polygon = autoware::universe_utils::toFootprint( + odometry_ptr_->pose.pose, base_to_front, base_to_rear, width); geometry_msgs::msg::Point nearest_point; double minimum_distance = std::numeric_limits::max(); @@ -392,18 +341,8 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamic { if (!object_ptr_ || !getUseDynamicObject()) return std::nullopt; - const auto transform_stamped = - getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); - - if (!transform_stamped) { - return std::nullopt; - } - const auto param = param_listener_->get_params(); - tf2::Transform tf_src2target; - tf2::fromMsg(transform_stamped.value().transform, tf_src2target); - // TODO(murooka) check computation cost geometry_msgs::msg::Point nearest_point; double minimum_distance = std::numeric_limits::max(); @@ -420,19 +359,13 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamic const double front_margin = object_param.surround_check_front_distance; const double side_margin = object_param.surround_check_side_distance; const double back_margin = object_param.surround_check_back_distance; - const auto ego_polygon = - createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); + const double base_to_front = vehicle_info_.max_longitudinal_offset_m + front_margin; + const double base_to_rear = vehicle_info_.rear_overhang_m + back_margin; + const double width = vehicle_info_.vehicle_width_m + side_margin * 2; + const auto ego_polygon = autoware::universe_utils::toFootprint( + odometry_ptr_->pose.pose, base_to_front, base_to_rear, width); - tf2::Transform tf_src2object; - tf2::fromMsg(object_pose, tf_src2object); - - geometry_msgs::msg::Pose transformed_object_pose; - tf2::toMsg(tf_src2target.inverse() * tf_src2object, transformed_object_pose); - - const auto object_polygon = - object.shape.type == Shape::POLYGON - ? createObjPolygon(transformed_object_pose, object.shape.footprint) - : createObjPolygon(transformed_object_pose, object.shape.dimensions); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object); const auto distance_to_object = bg::distance(ego_polygon, object_polygon); @@ -465,34 +398,32 @@ std::optional SurroundObstacleCheckerNode: return transform_stamped; } -bool SurroundObstacleCheckerNode::isStopRequired( - const bool is_obstacle_found, const bool is_vehicle_stopped) +auto SurroundObstacleCheckerNode::isStopRequired( + const bool is_obstacle_found, const bool is_vehicle_stopped, const State & state, + const std::optional & last_obstacle_found_time, + const double time_threshold) const -> std::pair> { if (!is_vehicle_stopped) { - return false; + return std::make_pair(false, std::nullopt); } if (is_obstacle_found) { - last_obstacle_found_time_ = std::make_shared(this->now()); - return true; + return std::make_pair(true, this->now()); } - if (state_ != State::STOP) { - return false; + if (state != State::STOP) { + return std::make_pair(false, std::nullopt); } - const auto param = param_listener_->get_params(); - // Keep stop state - if (last_obstacle_found_time_) { - const auto elapsed_time = this->now() - *last_obstacle_found_time_; - if (elapsed_time.seconds() <= param.state_clear_time) { - return true; + if (last_obstacle_found_time.has_value()) { + const auto elapsed_time = this->now() - last_obstacle_found_time.value(); + if (elapsed_time.seconds() <= time_threshold) { + return std::make_pair(true, last_obstacle_found_time.value()); } } - last_obstacle_found_time_ = {}; - return false; + return std::make_pair(false, std::nullopt); } } // namespace autoware::surround_obstacle_checker diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index 2e40eac3dfdfc..f04a3aba07baf 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -86,7 +86,10 @@ class SurroundObstacleCheckerNode : public rclcpp::Node const std::string & source, const std::string & target, const rclcpp::Time & stamp, double duration_sec) const; - bool isStopRequired(const bool is_obstacle_found, const bool is_vehicle_stopped); + auto isStopRequired( + const bool is_obstacle_found, const bool is_vehicle_stopped, const State & state, + const std::optional & last_obstacle_found_time, + const double time_threshold) const -> std::pair>; // ros mutable tf2_ros::Buffer tf_buffer_{get_clock()}; @@ -124,11 +127,14 @@ class SurroundObstacleCheckerNode : public rclcpp::Node // State Machine State state_ = State::PASS; - std::shared_ptr last_obstacle_found_time_; + std::optional last_obstacle_found_time_; std::unique_ptr logger_configure_; std::unordered_map label_map_; + +public: + friend class SurroundObstacleCheckerNodeTest; }; } // namespace autoware::surround_obstacle_checker diff --git a/planning/autoware_surround_obstacle_checker/test/test_surround_obstacle_checker.cpp b/planning/autoware_surround_obstacle_checker/test/test_surround_obstacle_checker.cpp new file mode 100644 index 0000000000000..c5fbb7958208d --- /dev/null +++ b/planning/autoware_surround_obstacle_checker/test/test_surround_obstacle_checker.cpp @@ -0,0 +1,129 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/node.hpp" + +#include +#include +#include + +#include + +namespace autoware::surround_obstacle_checker +{ +auto generateTestTargetNode() -> std::shared_ptr +{ + rclcpp::init(0, nullptr); + + auto node_options = rclcpp::NodeOptions{}; + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + + autoware::test_utils::updateNodeOptions( + node_options, + {autoware_test_utils_dir + "/config/test_common.param.yaml", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + ament_index_cpp::get_package_share_directory("autoware_surround_obstacle_checker") + + "/config/surround_obstacle_checker.param.yaml"}); + + return std::make_shared(node_options); +} + +class SurroundObstacleCheckerNodeTest : public ::testing::Test +{ +public: + SurroundObstacleCheckerNodeTest() : node_{generateTestTargetNode()} {} + + auto isStopRequired( + const bool is_obstacle_found, const bool is_vehicle_stopped, const State & state, + const std::optional & last_obstacle_found_time, + const double time_threshold) const -> std::pair> + { + return node_->isStopRequired( + is_obstacle_found, is_vehicle_stopped, state, last_obstacle_found_time, time_threshold); + } + +private: + std::shared_ptr node_; +}; + +TEST_F(SurroundObstacleCheckerNodeTest, isStopRequired) +{ + const auto LAST_STOP_TIME = rclcpp::Clock{RCL_ROS_TIME}.now(); + + using namespace std::literals::chrono_literals; + rclcpp::sleep_for(500ms); + + { + constexpr double THRESHOLD = 1.0; + const auto [is_stop, stop_time] = + isStopRequired(false, false, State::STOP, LAST_STOP_TIME, THRESHOLD); + EXPECT_FALSE(is_stop); + EXPECT_EQ(stop_time, std::nullopt); + } + + { + constexpr double THRESHOLD = 1.0; + const auto [is_stop, stop_time] = + isStopRequired(false, true, State::PASS, LAST_STOP_TIME, THRESHOLD); + EXPECT_FALSE(is_stop); + EXPECT_EQ(stop_time, std::nullopt); + } + + { + constexpr double THRESHOLD = 1.0; + const auto [is_stop, stop_time] = + isStopRequired(true, true, State::STOP, LAST_STOP_TIME, THRESHOLD); + + ASSERT_TRUE(stop_time.has_value()); + + const auto time_diff = rclcpp::Clock{RCL_ROS_TIME}.now() - stop_time.value(); + + EXPECT_TRUE(is_stop); + EXPECT_NEAR(time_diff.seconds(), 0.0, 1e-3); + } + + { + constexpr double THRESHOLD = 1.0; + const auto [is_stop, stop_time] = + isStopRequired(false, true, State::STOP, LAST_STOP_TIME, THRESHOLD); + + ASSERT_TRUE(stop_time.has_value()); + + const auto time_diff = rclcpp::Clock{RCL_ROS_TIME}.now() - stop_time.value(); + + EXPECT_TRUE(is_stop); + EXPECT_NEAR(time_diff.seconds(), 0.5, 1e-3); + } + + { + constexpr double THRESHOLD = 0.25; + const auto [is_stop, stop_time] = + isStopRequired(false, true, State::STOP, LAST_STOP_TIME, THRESHOLD); + EXPECT_FALSE(is_stop); + EXPECT_EQ(stop_time, std::nullopt); + } + + { + constexpr double THRESHOLD = 1.0; + const auto [is_stop, stop_time] = + isStopRequired(false, true, State::STOP, std::nullopt, THRESHOLD); + EXPECT_FALSE(is_stop); + EXPECT_EQ(stop_time, std::nullopt); + } + + rclcpp::shutdown(); +} +} // namespace autoware::surround_obstacle_checker diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml index aa3a207ea5be9..f5703d896d58f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Maxime CLEMENT + Alqudah Mohammad Apache License 2.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml index 41db5ade58a74..464951bc81008 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Maxime CLEMENT + Alqudah Mohammad Apache License 2.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp index 67aa41a5af7e5..1092047e65030 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -35,7 +35,7 @@ class PathDecisionState }; DecisionKind state{DecisionKind::NOT_DECIDED}; - rclcpp::Time stamp{}; + std::optional deciding_start_time{std::nullopt}; bool is_stable_safe{false}; std::optional safe_start_time{std::nullopt}; }; @@ -43,7 +43,7 @@ class PathDecisionState class PathDecisionStateController { public: - PathDecisionStateController() = default; + explicit PathDecisionStateController(rclcpp::Logger logger) : logger_(logger) {} /** * @brief update current state and save old current state to prev state @@ -62,11 +62,10 @@ class PathDecisionStateController PathDecisionState get_current_state() const { return current_state_; } - PathDecisionState get_prev_state() const { return prev_state_; } - private: + rclcpp::Logger logger_; + PathDecisionState current_state_{}; - PathDecisionState prev_state_{}; /** * @brief update current state and save old current state to prev state diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 12d564237db3c..70c9fd528b6a5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -321,7 +321,7 @@ class GoalPlannerModule : public SceneModuleInterface // context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess() std::optional context_data_{std::nullopt}; // path_decision_controller is updated in updateData(), and used in plan() - PathDecisionStateController path_decision_controller_{}; + PathDecisionStateController path_decision_controller_{getLogger()}; std::unique_ptr last_approval_data_{nullptr}; // approximate distance from the start point to the end point of pull_over. diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index 34743ae5fbf5f..ee370fd96b9a0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -37,8 +37,9 @@ class FreespacePullOver : public PullOverPlannerBase PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) override; protected: const double velocity_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index 2dfcfb3dc6e9e..89181b258fbea 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -43,8 +43,9 @@ class GeometricPullOver : public PullOverPlannerBase Pose getCl() const { return planner_.getCl(); } std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) override; std::vector generatePullOverPaths( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 47367164b2517..059d76530915d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -15,6 +15,7 @@ #pragma once #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "autoware/behavior_path_planner_common/data_manager.hpp" #include @@ -41,19 +42,20 @@ struct PullOverPath { public: static std::optional create( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const PullOverPlannerType & type, const size_t id, const std::vector & partial_paths, const Pose & start_pose, - const Pose & end_pose, + const GoalCandidate & modified_goal_pose, const std::vector> & pairs_terminal_velocity_and_accel); PullOverPath(const PullOverPath & other); PullOverPath & operator=(const PullOverPath & other) = default; PullOverPlannerType type() const { return type_; } - size_t goal_id() const { return goal_id_; } + size_t goal_id() const { return modified_goal_pose_.id; } size_t id() const { return id_; } Pose start_pose() const { return start_pose_; } - Pose end_pose() const { return end_pose_; } + Pose end_pose() const { return modified_goal_pose_.goal_pose; } + GoalCandidate modified_goal_pose() const { return modified_goal_pose_; } std::vector & partial_paths() { return partial_paths_; } const std::vector & partial_paths() const { return partial_paths_; } @@ -86,19 +88,18 @@ struct PullOverPath private: PullOverPath( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, - const Pose & start_pose, const Pose & end_pose, - const std::vector & partial_paths, const PathWithLaneId & full_path, - const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, + const PullOverPlannerType & type, const size_t id, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector & partial_paths, + const PathWithLaneId & full_path, const PathWithLaneId & parking_path, + const std::vector & full_path_curvatures, const std::vector & parking_path_curvatures, const double full_path_max_curvature, const double parking_path_max_curvature, const std::vector> & pairs_terminal_velocity_and_accel); PullOverPlannerType type_; - size_t goal_id_; + GoalCandidate modified_goal_pose_; size_t id_; Pose start_pose_; - Pose end_pose_; std::vector partial_paths_; PathWithLaneId full_path_; @@ -126,8 +127,9 @@ class PullOverPlannerBase virtual PullOverPlannerType getPlannerType() const = 0; virtual std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) = 0; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) = 0; protected: const autoware::vehicle_info_utils::VehicleInfo vehicle_info_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index 9baceb4430dec..bfb0173874784 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -36,8 +36,9 @@ class ShiftPullOver : public PullOverPlannerBase const LaneDepartureChecker & lane_departure_checker); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) override; protected: PathWithLaneId generateReferencePath( @@ -46,10 +47,10 @@ class ShiftPullOver : public PullOverPlannerBase std::optional cropPrevModulePath( const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, + const GoalCandidate & goal_candidate, const size_t id, + const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, - const double lateral_jerk) const; + const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const; static double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); static std::vector splineTwoPoints( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 55cbd4c0e29d5..065e27b47227e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -303,8 +303,7 @@ void GoalPlannerModule::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { const auto pull_over_path = planner->plan( - goal_candidate.id, path_candidates.size(), local_planner_data, previous_module_output, - goal_candidate.goal_pose); + goal_candidate, path_candidates.size(), local_planner_data, previous_module_output); if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority @@ -803,9 +802,8 @@ bool GoalPlannerModule::planFreespacePath( continue; } const auto freespace_path = freespace_planner_->plan( - goal_candidate.id, 0, planner_data, - BehaviorModuleOutput{}, // NOTE: not used so passing {} is OK - goal_candidate.goal_pose); + goal_candidate, 0, planner_data, BehaviorModuleOutput{} // NOTE: not used so passing {} is OK + ); if (!freespace_path) { continue; } @@ -1824,7 +1822,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & return ignore_signal_.value() == id; }; - const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) { return is_ignore ? std::make_optional(id) : std::nullopt; }; @@ -2162,7 +2160,7 @@ static std::vector filterOb const auto & target_object_types = params->object_types_to_check; PredictedObjects filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( - *objects, ignore_object_velocity_threshold, false); + *objects, ignore_object_velocity_threshold, true); utils::path_safety_checker::filterObjectsByClass(filtered_objects, target_object_types); @@ -2643,7 +2641,6 @@ void PathDecisionStateController::transit_state( found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, pull_over_path, ego_polygons_expanded); - prev_state_ = current_state_; current_state_ = next_state; } @@ -2658,10 +2655,7 @@ PathDecisionState PathDecisionStateController::get_next_state( const std::optional & pull_over_path_opt, std::vector & ego_polygons_expanded) const { - auto next_state = prev_state_; - - // update timestamp - next_state.stamp = now; + auto next_state = current_state_; // update safety if (!parameters.safety_check_params.enable_safety_check) { @@ -2684,7 +2678,6 @@ PathDecisionState PathDecisionStateController::get_next_state( // Once this function returns true, it will continue to return true thereafter if (next_state.state == PathDecisionState::DecisionKind::DECIDED) { - next_state.state = PathDecisionState::DecisionKind::DECIDED; return next_state; } @@ -2699,12 +2692,16 @@ PathDecisionState PathDecisionStateController::get_next_state( // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " + "approval"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; } const auto & current_path = pull_over_path.getCurrentPath(); - if (prev_state_.state == PathDecisionState::DecisionKind::DECIDING) { + if (current_state_.state == PathDecisionState::DecisionKind::DECIDING) { const double hysteresis_factor = 0.9; // check goal pose collision @@ -2712,7 +2709,9 @@ PathDecisionState PathDecisionStateController::get_next_state( modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, planner_data, static_target_objects)) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } @@ -2727,24 +2726,36 @@ PathDecisionState PathDecisionStateController::get_next_state( /*extract_static_objects=*/false, parameters.maximum_deceleration, parameters.object_recognition_collision_check_max_extra_stopping_margin, ego_polygons_expanded, true)) { + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } if (enable_safety_check && !next_state.is_stable_safe) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; } // if enough time has passed since deciding status starts, transition to DECIDED constexpr double check_collision_duration = 1.0; - const double elapsed_time_from_deciding = (now - prev_state_.stamp).seconds(); + const double elapsed_time_from_deciding = + (now - current_state_.deciding_start_time.value()).seconds(); if (elapsed_time_from_deciding > check_collision_duration) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); next_state.state = PathDecisionState::DecisionKind::DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } // if enough time has NOT passed since deciding status starts, keep DECIDING + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", + elapsed_time_from_deciding); return next_state; } @@ -2766,10 +2777,15 @@ PathDecisionState PathDecisionStateController::get_next_state( // if object recognition for path collision check is enabled, transition to DECIDING to check // collision for a certain period of time. Otherwise, transition to DECIDED directly. if (parameters.use_object_recognition) { - next_state.state = PathDecisionState::DecisionKind::DECIDED; + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " + "period of time"); + next_state.state = PathDecisionState::DecisionKind::DECIDING; + next_state.deciding_start_time = now; return next_state; } - return {PathDecisionState::DecisionKind::DECIDED, now}; + return {PathDecisionState::DecisionKind::DECIDED, std::nullopt}; } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index dbdac08c8778c..452090571ac45 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -57,8 +57,9 @@ FreespacePullOver::FreespacePullOver( } std::optional FreespacePullOver::plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) { const Pose & current_pose = planner_data->self_odometry->pose.pose; @@ -67,6 +68,7 @@ std::optional FreespacePullOver::plan( // offset goal pose to make straight path near goal for improving parking precision // todo: support straight path when using back constexpr double straight_distance = 1.0; + const auto & goal_pose = modified_goal_pose.goal_pose; const Pose end_pose = use_back_ ? goal_pose : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); @@ -146,7 +148,7 @@ std::optional FreespacePullOver::plan( } auto pull_over_path_opt = PullOverPath::create( - getPlannerType(), goal_id, id, partial_paths, current_pose, goal_pose, + getPlannerType(), id, partial_paths, current_pose, modified_goal_pose, pairs_terminal_velocity_and_accel); if (!pull_over_path_opt) { return {}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index 6c4aee5b96abf..09be040019338 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -38,11 +38,13 @@ GeometricPullOver::GeometricPullOver( } std::optional GeometricPullOver::plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) { const auto & route_handler = planner_data->route_handler; + const auto & goal_pose = modified_goal_pose.goal_pose; // prepare road nad shoulder lanes const auto road_lanes = utils::getExtendedCurrentLanes( planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, @@ -73,8 +75,8 @@ std::optional GeometricPullOver::plan( if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {}; auto pull_over_path_opt = PullOverPath::create( - getPlannerType(), goal_id, id, planner_.getPaths(), planner_.getStartPose(), - planner_.getArcEndPose(), planner_.getPairsTerminalVelocityAndAccel()); + getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose, + planner_.getPairsTerminalVelocityAndAccel()); if (!pull_over_path_opt) { return {}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp index f6535e7adb8f8..63610f5ac31f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -18,8 +18,9 @@ namespace autoware::behavior_path_planner { std::optional PullOverPath::create( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, - const std::vector & partial_paths, const Pose & start_pose, const Pose & end_pose, + const PullOverPlannerType & type, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector> & pairs_terminal_velocity_and_accel) { if (partial_paths.empty()) { @@ -73,20 +74,19 @@ std::optional PullOverPath::create( double parking_path_max_curvature{}; std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path); std::tie(parking_path_curvatures, parking_path_max_curvature) = - calculateCurvaturesAndMax(full_path); + calculateCurvaturesAndMax(parking_path); return PullOverPath( - type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path, + type, id, start_pose, modified_goal_pose, partial_paths, full_path, parking_path, full_path_curvatures, parking_path_curvatures, full_path_max_curvature, parking_path_max_curvature, pairs_terminal_velocity_and_accel); } PullOverPath::PullOverPath(const PullOverPath & other) : type_(other.type_), - goal_id_(other.goal_id_), + modified_goal_pose_(other.modified_goal_pose_), id_(other.id_), start_pose_(other.start_pose_), - end_pose_(other.end_pose_), partial_paths_(other.partial_paths_), full_path_(other.full_path_), parking_path_(other.parking_path_), @@ -100,18 +100,17 @@ PullOverPath::PullOverPath(const PullOverPath & other) } PullOverPath::PullOverPath( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, const Pose & start_pose, - const Pose & end_pose, const std::vector & partial_paths, + const PullOverPlannerType & type, const size_t id, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector & partial_paths, const PathWithLaneId & full_path, const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, const std::vector & parking_path_curvatures, const double full_path_max_curvature, const double parking_path_max_curvature, const std::vector> & pairs_terminal_velocity_and_accel) : type_(type), - goal_id_(goal_id), + modified_goal_pose_(modified_goal_pose), id_(id), start_pose_(start_pose), - end_pose_(end_pose), partial_paths_(partial_paths), full_path_(full_path), parking_path_(parking_path), diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 645d74b6385da..30f250634c028 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -35,8 +35,9 @@ ShiftPullOver::ShiftPullOver( { } std::optional ShiftPullOver::plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -59,7 +60,7 @@ std::optional ShiftPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generatePullOverPath( - goal_id, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose, + modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; @@ -127,14 +128,16 @@ std::optional ShiftPullOver::cropPrevModulePath( } std::optional ShiftPullOver::generatePullOverPath( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, + const GoalCandidate & goal_candidate, const size_t id, + const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, - const double lateral_jerk) const + const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; const double after_shift_straight_distance = parameters_.after_shift_straight_distance; + const auto & goal_pose = goal_candidate.goal_pose; + // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); @@ -256,8 +259,8 @@ std::optional ShiftPullOver::generatePullOverPath( // set pull over path auto pull_over_path_opt = PullOverPath::create( - getPlannerType(), goal_id, id, {shifted_path.path}, path_shifter.getShiftLines().front().start, - path_shifter.getShiftLines().front().end, {std::make_pair(pull_over_velocity, 0)}); + getPlannerType(), id, {shifted_path.path}, path_shifter.getShiftLines().front().start, + goal_candidate, {std::make_pair(pull_over_velocity, 0)}); if (!pull_over_path_opt) { return {}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 18a797976161c..02280f7ffa697 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -195,7 +195,7 @@ if (max_lane_change_length > ego's distance to the end of the current lanes.) t stop endif -if (isVehicleStuck(current_lanes)) then (yes) +if ego is stuck in the current lanes then (yes) :Return **sampled acceleration values**; stop else (no) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index f76d776150875..cc06d0b114d7b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -89,10 +89,6 @@ class NormalLaneChange : public LaneChangeBase bool isRequiredStop(const bool is_trailing_object) override; - bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const override; - bool hasFinishedLaneChange() const override; bool isAbleToReturnCurrentLane() const override; @@ -116,8 +112,7 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_current_turn_signal_info() const final; protected: - lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const override; + lanelet::ConstLanelets get_lane_change_lanes(const lanelet::ConstLanelets & current_lanes) const; int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; @@ -127,9 +122,7 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - std::vector calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + std::vector calc_prepare_durations() const; lane_change::TargetObjects getTargetObjects( const FilteredByLanesExtendedObjects & predicted_objects, @@ -142,30 +135,28 @@ class NormalLaneChange : public LaneChangeBase FilteredByLanesObjects filterObjectsByLanelets( const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; - PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const override; + bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const override; PathWithLaneId getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, const double lane_changing_velocity, const double buffer_for_next_lane_change) const; - bool hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; + std::vector get_prepare_metrics() const; + std::vector get_lane_changing_metrics( + const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, + const double shift_length, const double dist_to_reg_element) const; bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; LaneChangePath get_candidate_path( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double target_lane_length, const double shift_length, - const double next_lc_buffer, const bool is_goal_in_route) const; + const Pose & lc_start_pose, const double shift_length) const; bool check_candidate_path_safety( - const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const double lane_change_buffer, const bool is_stuck) const; + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, @@ -189,15 +180,9 @@ class NormalLaneChange : public LaneChangeBase const std::vector & ego_predicted_path, const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const; - //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. - //! @param obstacle_check_distance Distance to check ahead for any objects that might be - //! obstructing ego path. It makes sense to use values like the maximum lane change distance. - bool isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; - double get_max_velocity_for_safety_check() const; - bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + bool is_ego_stuck() const; /** * @brief Checks if the given pose is a valid starting point for a lane change. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 72a40caca1d6a..c124353e2873a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -112,10 +112,6 @@ class LaneChangeBase virtual PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( PathSafetyStatus approve_path_safety_status) = 0; - virtual bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const = 0; - virtual bool isStoppedAtRedTrafficLight() const = 0; virtual bool calcAbortPath() = 0; @@ -238,17 +234,13 @@ class LaneChangeBase protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; - virtual PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const = 0; + virtual bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; - virtual lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; - virtual TurnSignalInfo get_terminal_turn_signal_info() const = 0; TurnSignalInfo get_turn_signal(const Pose & start, const Pose & end) const diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index fc5e78e44b77f..e4f70f54a8437 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -338,7 +338,13 @@ struct TransientData double max_prepare_length{ std::numeric_limits::max()}; // maximum prepare length, starting from ego's base link + double target_lane_length{std::numeric_limits::min()}; + + lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes + lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes + bool is_ego_near_current_terminal_start{false}; + bool is_ego_stuck{false}; }; using RouteHandlerPtr = std::shared_ptr; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 363fa970f54c4..3219dc7e777f1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -51,6 +51,7 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; +using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -58,6 +59,8 @@ using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using tier4_planning_msgs::msg::PathWithLaneId; +bool is_mandatory_lane_change(const ModuleType lc_type); + double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); @@ -79,9 +82,7 @@ std::vector replaceWithSortedIds( const std::vector & original_lane_ids, const std::vector> & sorted_lane_ids); -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); lanelet::ConstLanelets getTargetNeighborLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, @@ -104,12 +105,9 @@ ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length); -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, - const double next_lane_change_buffer); +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval); std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, @@ -123,9 +121,9 @@ double getLateralShift(const LaneChangePath & path); CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); -std::optional getLaneChangeTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType type, const Direction & direction); + +std::optional get_lane_change_target_lane( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes); std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, @@ -145,8 +143,7 @@ bool isParkedObject( bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug); + const std::vector & objects, CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 0b647acd44268..09550a135ba3c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -76,8 +76,8 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); - module_type_->update_transient_data(); module_type_->update_filtered_objects(); + module_type_->update_transient_data(); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index a91aaba86bb1a..5a9ee485320b8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -69,7 +69,7 @@ void NormalLaneChange::update_lanes(const bool is_approved) return; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + const auto target_lanes = get_lane_change_lanes(current_lanes); if (target_lanes.empty()) { return; } @@ -129,9 +129,21 @@ void NormalLaneChange::update_transient_data() transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + transient_data.target_lane_length = + lanelet::utils::getLaneletLength2d(common_data_ptr_->lanes_ptr->target); + + transient_data.current_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->current, common_data_ptr_->get_ego_pose()); + + transient_data.target_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->target, common_data_ptr_->get_ego_pose()); + transient_data.is_ego_near_current_terminal_start = transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + updateStopTime(); + transient_data.is_ego_stuck = is_ego_stuck(); + RCLCPP_DEBUG( logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); RCLCPP_DEBUG( @@ -159,15 +171,13 @@ void NormalLaneChange::update_filtered_objects() void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - const auto arclength_start = lanelet::utils::getArcCoordinates(get_target_lanes(), getEgoPose()); - status_.start_distance = arclength_start.length; + status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -233,10 +243,7 @@ bool NormalLaneChange::is_near_regulatory_element() const { if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) return false; - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; - - if (dist_to_terminal_start <= max_prepare_length) return false; + if (common_data_ptr_->transient_data.is_ego_near_current_terminal_start) return false; const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; @@ -244,8 +251,9 @@ bool NormalLaneChange::is_near_regulatory_element() const RCLCPP_DEBUG(logger_, "Stop time is over threshold. Ignore crosswalk and intersection checks."); } - return max_prepare_length > utils::lane_change::get_distance_to_next_regulatory_element( - common_data_ptr_, only_tl, only_tl); + return common_data_ptr_->transient_data.max_prepare_length > + utils::lane_change::get_distance_to_next_regulatory_element( + common_data_ptr_, only_tl, only_tl); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -337,7 +345,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const } const auto terminal_path = - calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + calcTerminalLaneChangePath(current_lanes, get_lane_change_lanes(current_lanes)); if (!terminal_path) { RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); return prev_module_output_; @@ -593,12 +601,13 @@ std::optional NormalLaneChange::extendPath() } auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - const auto dist_in_target = lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); + const auto & transient_data = common_data_ptr_->transient_data; const auto forward_path_length = getCommonParam().forward_path_length; - if ((target_lane_length - dist_in_target.length) > forward_path_length) { + if ( + (transient_data.target_lane_length - transient_data.target_lanes_ego_arc.length) > + forward_path_length) { return std::nullopt; } @@ -627,15 +636,7 @@ std::optional NormalLaneChange::extendPath() return getRouteHandler()->getGoalPose(); } - Pose back_pose; - const auto back_point = - lanelet::utils::conversion::toGeomMsgPt(next_lane.centerline2d().back()); - const double front_yaw = lanelet::utils::getLaneletAngle(next_lane, back_point); - back_pose.position = back_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, front_yaw); - back_pose.orientation = tf2::toMsg(tf_quat); - return back_pose; + return utils::to_geom_msg_pose(next_lane.centerline2d().back(), next_lane); }); const auto dist_to_target_pose = @@ -646,6 +647,7 @@ std::optional NormalLaneChange::extendPath() return getRouteHandler()->getCenterLinePath( target_lanes, dist_to_end_of_path, dist_to_target_pose); } + void NormalLaneChange::resetParameters() { is_abort_path_approved_ = false; @@ -662,11 +664,10 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto & pose = getEgoPose(); const auto & current_lanes = get_current_lanes(); const auto & shift_line = status_.lane_change_path.info.shift_line; const auto & shift_path = status_.lane_change_path.shifted_path; - const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + const auto current_shift_length = common_data_ptr_->transient_data.current_lanes_ego_arc.distance; constexpr bool is_driving_forward = true; // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's // current lane, lane change is different, so we set this flag to false. @@ -680,8 +681,8 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const return new_signal; } -lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const +lanelet::ConstLanelets NormalLaneChange::get_lane_change_lanes( + const lanelet::ConstLanelets & current_lanes) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { @@ -690,69 +691,27 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( // Get lane change lanes const auto & route_handler = getRouteHandler(); - const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( - *getRouteHandler(), current_lanes, type_, direction); + const auto lane_change_lane = + utils::lane_change::get_lane_change_target_lane(common_data_ptr_, current_lanes); if (!lane_change_lane) { return {}; } - const auto front_pose = std::invoke([&lane_change_lane]() { - const auto & p = lane_change_lane->centerline().front(); - const auto front_point = lanelet::utils::conversion::toGeomMsgPt(p); - const auto front_yaw = lanelet::utils::getLaneletAngle(*lane_change_lane, front_point); - geometry_msgs::msg::Pose front_pose; - front_pose.position = front_point; - tf2::Quaternion quat; - quat.setRPY(0, 0, front_yaw); - front_pose.orientation = tf2::toMsg(quat); - return front_pose; - }); - const auto forward_length = std::invoke([&]() { + const auto front_pose = + utils::to_geom_msg_pose(lane_change_lane->centerline().front(), *lane_change_lane); const auto signed_distance = utils::getSignedDistance(front_pose, getEgoPose(), current_lanes); const auto forward_path_length = planner_data_->parameters.forward_path_length; - if (signed_distance <= 0.0) { - return forward_path_length; - } - - return signed_distance + forward_path_length; + return forward_path_length + std::max(signed_distance, 0.0); }); + const auto backward_length = lane_change_parameters_->backward_lane_length; return route_handler->getLaneletSequence( lane_change_lane.value(), getEgoPose(), backward_length, forward_length); } -bool NormalLaneChange::isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const -{ - if (current_lanes.empty()) { - return false; - } - - const auto & route_handler = getRouteHandler(); - const auto & current_pose = getEgoPose(); - - // TODO(Azu) fully change to transient data - const auto distance_to_lane_change_end = std::invoke([&]() { - auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); - - if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { - distance_to_end = std::min( - distance_to_end, - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); - } - - return std::max(0.0, distance_to_end) - - common_data_ptr_->transient_data.current_dist_buffer.min; - }); - - lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; - return distance_to_lane_change_end < threshold; -} - bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); @@ -789,7 +748,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const return false; } - const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + const auto & arc_length = common_data_ptr_->transient_data.target_lanes_ego_arc; const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; @@ -956,6 +915,7 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons std::vector NormalLaneChange::sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { + // TODO(Azu): sampler should work even when we're not approaching terminal if (prev_module_output_.path.points.empty()) { return {}; } @@ -979,7 +939,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( const auto current_max_dist_buffer = calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - if (current_max_dist_buffer > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (current_max_dist_buffer > common_data_ptr_->transient_data.dist_to_terminal_end) { RCLCPP_DEBUG( logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); @@ -988,7 +948,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (isVehicleStuck(current_lanes)) { + if (common_data_ptr_->transient_data.is_ego_stuck) { auto clock = rclcpp::Clock(RCL_ROS_TIME); RCLCPP_INFO_THROTTLE( logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, @@ -1016,52 +976,72 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } -std::vector NormalLaneChange::calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +std::vector NormalLaneChange::calc_prepare_durations() const { - const auto base_link2front = planner_data_->parameters.base_link2front; - const auto threshold = - lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + const auto threshold = common_data_ptr_->bpp_param_ptr->base_link2front + + lc_param_ptr->min_length_for_turn_signal_activation; + const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; + + // TODO(Azu) this check seems to cause scenario failures. + if (common_data_ptr_->transient_data.dist_to_terminal_start >= threshold) { + return {max_prepare_duration}; + } std::vector prepare_durations; constexpr double step = 0.5; - for (double duration = lane_change_parameters_->lane_change_prepare_duration; duration >= 0.0; - duration -= step) { + for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) { prepare_durations.push_back(duration); - if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { - break; - } } return prepare_durations; } -PathWithLaneId NormalLaneChange::getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const +bool NormalLaneChange::get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const { - if (current_lanes.empty()) { - return PathWithLaneId(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto backward_path_length = common_data_ptr_->bpp_param_ptr->backward_path_length; + + if (current_lanes.empty() || target_lanes.empty()) { + return false; } - auto prepare_segment = prev_module_output_.path; + prepare_segment = prev_module_output_.path; const size_t current_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - return prepare_segment; + if (prepare_segment.points.empty()) return false; + + const auto & lc_start_pose = prepare_segment.points.back().point.pose; + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr_, lc_start_pose)) return false; + + // lane changing start is at the end of prepare segment + const auto target_length_from_lane_change_start_pose = + utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { + throw std::logic_error("lane change start is behind target lanelet!"); + } + + return true; } lane_change::TargetObjects NormalLaneChange::getTargetObjects( const FilteredByLanesExtendedObjects & filtered_objects, - const lanelet::ConstLanelets & current_lanes) const + [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; - const auto is_stuck = isVehicleStuck(current_lanes); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; - if (chk_obj_in_curr_lanes || is_stuck) { + if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { leading_objects.insert( leading_objects.end(), filtered_objects.current_lane.begin(), filtered_objects.current_lane.end()); @@ -1106,8 +1086,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return {}; } - const auto path = - route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & path = common_data_ptr_->current_lanes_path; auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); @@ -1333,110 +1312,89 @@ PathWithLaneId NormalLaneChange::getTargetSegment( return target_segment; } -bool NormalLaneChange::hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, [[maybe_unused]] const Direction direction) const +std::vector NormalLaneChange::get_prepare_metrics() const { - if (target_lanes.empty()) { - return false; - } + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto current_velocity = getEgoVelocity(); - const auto current_pose = getEgoPose(); - const auto & route_handler = getRouteHandler(); - const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); - const auto minimum_lane_change_length_to_preferred_lane = - common_data_ptr_->transient_data.next_dist_buffer.min; + // get sampling acceleration values + const auto longitudinal_acc_sampling_values = + sampleLongitudinalAccValues(current_lanes, target_lanes); - const double lane_change_length = path.info.length.sum(); - if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { - return false; - } + const auto prepare_durations = calc_prepare_durations(); - const auto goal_pose = route_handler->getGoalPose(); - if ( - route_handler->isInGoalRouteSection(current_lanes.back()) && - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getSignedDistance(current_pose, goal_pose, current_lanes)) { - return false; - } + RCLCPP_DEBUG( + logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", + prepare_durations.size(), longitudinal_acc_sampling_values.size()); - // return if there are no target lanes - if ( - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getDistanceToEndOfLane(current_pose, target_lanes)) { - return false; - } + const auto dist_to_target_start = + calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); + return calculation::calc_prepare_phase_metrics( + common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, + dist_to_target_start, common_data_ptr_->transient_data.dist_to_terminal_start); +} - return true; +std::vector NormalLaneChange::get_lane_changing_metrics( + const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metric, + const double shift_length, const double dist_to_reg_element) const +{ + const auto & route_handler = getRouteHandler(); + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto & transient_data = common_data_ptr_->transient_data; + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, + prep_segment.points.back().point.pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = + transient_data.is_ego_near_current_terminal_start + ? transient_data.dist_to_terminal_end - prep_metric.length + : std::min(transient_data.dist_to_terminal_end, dist_to_reg_element) - prep_metric.length; + auto target_lane_buffer = lane_change_parameters_->lane_change_finish_judge_buffer + + transient_data.next_dist_buffer.min; + if (std::abs(route_handler->getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { + target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; + } + max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); + return max_length; + }); + + const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps; + return calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + prep_metric.sampled_lon_accel, max_lane_changing_length); } bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { lane_change_debug_.collision_check_objects.clear(); - const auto & current_lanes = get_current_lanes(); - const auto & target_lanes = get_target_lanes(); - const auto is_stuck = isVehicleStuck(current_lanes); - - const auto is_empty = [&](const auto & data, const auto & s) { - if (!data.empty()) return false; - RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); - return true; - }; + if (!common_data_ptr_->is_lanes_available()) { + RCLCPP_WARN(logger_, "lanes are not available. Not expected."); + return false; + } - const auto target_lane_neighbors_polygon_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if ( - is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || - is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { + if (common_data_ptr_->lanes_polygon_ptr->target_neighbor.empty()) { + RCLCPP_WARN(logger_, "target_lane_neighbors_polygon_2d is empty. Not expected."); return false; } - const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); - // get velocity const auto current_velocity = getEgoVelocity(); - - // get sampling acceleration values - const auto longitudinal_acc_sampling_values = - sampleLongitudinalAccValues(current_lanes, target_lanes); - - const auto is_goal_in_route = common_data_ptr_->lanes_ptr->target_lane_in_goal_section; - - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - - const auto dist_to_target_start = - calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto dist_to_terminal_end = common_data_ptr_->transient_data.dist_to_terminal_end; - const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; - - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); - const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); + const auto prepare_phase_metrics = get_prepare_metrics(); candidate_paths.reserve( - longitudinal_acc_sampling_values.size() * lane_change_parameters_->lateral_acc_sampling_num * - prepare_durations.size()); - - RCLCPP_DEBUG( - logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", - prepare_durations.size(), longitudinal_acc_sampling_values.size()); + prepare_phase_metrics.size() * lane_change_parameters_->lateral_acc_sampling_num); const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; const auto dist_to_next_regulatory_element = utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - - const auto prepare_phase_metrics = calculation::calc_prepare_phase_metrics( - common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, - dist_to_target_start, dist_to_terminal_start); auto check_length_diff = [&](const double prep_length, const double lc_length, const bool check_lc) { @@ -1452,10 +1410,10 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) }; for (const auto & prep_metric : prepare_phase_metrics) { - const auto debug_print = [&](const auto & s) { + const auto debug_print = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s, prep_metric.duration, - prep_metric.actual_lon_accel, prep_metric.length); + logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(), + prep_metric.duration, prep_metric.actual_lon_accel, prep_metric.length); }; if (!check_length_diff(prep_metric.length, 0.0, false)) { @@ -1463,59 +1421,26 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) continue; } - auto prepare_segment = - getPrepareSegment(current_lanes, common_parameters.backward_path_length, prep_metric.length); - - if (prepare_segment.points.empty()) { - debug_print("prepare segment is empty...? Unexpected."); - continue; - } - - // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is - // larger than distance to target lane start - if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { - debug_print( - "Reject: lane changing start point is not within the preferred lanes or its neighbors"); - continue; - } - - // lane changing start is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); - - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - debug_print("lane change start is behind target lanelet!"); + PathWithLaneId prepare_segment; + try { + if (!get_prepare_segment(prepare_segment, prep_metric.length)) { + debug_print("Reject: failed to get valid prepare segment!"); + continue; + } + } catch (const std::exception & e) { + debug_print(e.what()); break; } debug_print("Prepare path satisfy constraints"); + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( - common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); - - const auto max_lane_changing_length = std::invoke([&]() { - double max_length = - dist_to_terminal_start > max_prepare_length - ? std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length - : dist_to_terminal_end - prep_metric.length; - auto target_lane_buffer = - lane_change_parameters_->lane_change_finish_judge_buffer + next_min_dist_buffer; - if (std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { - target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; - } - max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); - return max_length; - }); - - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( - common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, - prep_metric.sampled_lon_accel, max_lane_changing_length); + const auto lane_changing_metrics = get_lane_changing_metrics( + prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); @@ -1535,7 +1460,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) try { candidate_path = get_candidate_path( prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, - target_lane_length, shift_length, next_min_dist_buffer, is_goal_in_route); + shift_length); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); continue; @@ -1543,20 +1468,16 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) candidate_paths.push_back(candidate_path); - bool is_safe = false; try { - is_safe = check_candidate_path_safety( - candidate_path, target_objects, current_min_dist_buffer, is_stuck); + if (check_candidate_path_safety(candidate_path, target_objects)) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; + } } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); return false; } - if (is_safe) { - debug_print_lat("ACCEPT!!!: it is valid and safe!"); - return true; - } - debug_print_lat("Reject: sampled path is not safe."); } } @@ -1568,19 +1489,15 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) LaneChangePath NormalLaneChange::get_candidate_path( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double target_lane_length, const double shift_length, - const double next_lc_buffer, const bool is_goal_in_route) const + const Pose & lc_start_pose, const double shift_length) const { const auto & route_handler = *getRouteHandler(); - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto & forward_path_length = planner_data_->parameters.forward_path_length; const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lc_start_pose, target_lane_length, lc_metrics.length, - forward_path_length, resample_interval, is_goal_in_route, next_lc_buffer); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lc_start_pose, lc_metrics.length, resample_interval); if (target_lane_reference_path.points.empty()) { throw std::logic_error("target_lane_reference_path is empty!"); @@ -1605,7 +1522,10 @@ LaneChangePath NormalLaneChange::get_candidate_path( throw std::logic_error("failed to generate candidate path!"); } - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction_)) { + if ( + candidate_path.value().info.length.sum() + + common_data_ptr_->transient_data.next_dist_buffer.min > + common_data_ptr_->transient_data.dist_to_terminal_end) { throw std::logic_error("invalid candidate path length!"); } @@ -1613,13 +1533,13 @@ LaneChangePath NormalLaneChange::get_candidate_path( } bool NormalLaneChange::check_candidate_path_safety( - const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const double lane_change_buffer, const bool is_stuck) const + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { + const auto is_stuck = common_data_ptr_->transient_data.is_ego_stuck; if ( !is_stuck && !utils::lane_change::passed_parked_objects( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, - lane_change_buffer, lane_change_debug_.collision_check_objects)) { + lane_change_debug_.collision_check_objects)) { throw std::logic_error( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); } @@ -1669,9 +1589,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( } const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto forward_path_length = common_parameters.forward_path_length; const auto minimum_lane_changing_velocity = lane_change_parameters_->minimum_lane_changing_velocity; @@ -1680,10 +1598,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = @@ -1722,6 +1637,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; const auto target_segment = getTargetSegment( target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, minimum_lane_changing_velocity, next_min_dist_buffer); @@ -1752,10 +1668,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( current_min_dist_buffer, minimum_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, - current_min_dist_buffer, forward_path_length, resample_interval, is_goal_in_route, - next_min_dist_buffer); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lane_changing_start_pose.value(), current_min_dist_buffer, resample_interval); if (target_lane_reference_path.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); @@ -1796,11 +1710,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, current_min_dist_buffer, - debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); @@ -1892,10 +1803,9 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( - isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && - isAbleToStopSafely() && is_trailing_object) { + common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() && + is_trailing_object) { current_lane_change_state_ = LaneChangeStates::Stop; return true; } @@ -2196,63 +2106,6 @@ bool NormalLaneChange::is_collided( return !is_collided; } -// Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes -bool NormalLaneChange::isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // Ego is still moving, not in stuck - if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { - RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); - return false; - } - - // Ego is just stopped, not sure it is in stuck yet. - if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); - return false; - } - - // Check if any stationary object exist in obstacle_check_distance - using lanelet::utils::getArcCoordinates; - const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; - - for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { - const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point - - // Note: it needs chattering prevention. - if (std::abs(object.initial_twist.linear.x) > 0.3) { // check if stationary - continue; - } - - const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; - if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { - RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); - return true; // Stationary object is in front of ego. - } - } - - // Check if Ego is in terminal of current lanes - const auto & route_handler = getRouteHandler(); - const double distance_to_terminal = - route_handler->isInGoalRouteSection(current_lanes.back()) - ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) - : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double terminal_judge_buffer = current_min_dist_buffer + stop_point_buffer + 1.0; - if (distance_to_terminal < terminal_judge_buffer) { - return true; - } - - // No stationary objects found in obstacle_check_distance and Ego is not in terminal of current - RCLCPP_DEBUG( - logger_, - "No stationary objects found in obstacle_check_distance and Ego is not in " - "terminal of current lanes"); - return false; -} - double NormalLaneChange::get_max_velocity_for_safety_check() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -2265,35 +2118,56 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const return getCommonParam().max_vel; } -bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::is_ego_stuck() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (current_lanes.empty()) { - lane_change_debug_.is_stuck = false; - return false; // can not check + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + + if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->stop_velocity_threshold) { + RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); + return false; } - const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - const auto current_max_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.max; - const auto rss_dist = calcRssDistance( - 0.0, lane_change_parameters_->minimum_lane_changing_velocity, - lane_change_parameters_->rss_params); + // Ego is just stopped, not sure it is in stuck yet. + if (getStopTime() < lc_param_ptr->stop_time_threshold) { + RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); + return false; + } + + // Check if any stationary object exist in obstacle_check_distance + const auto & current_lanes_path = common_data_ptr_->current_lanes_path; + const auto & ego_pose = common_data_ptr_->get_ego_pose(); + const auto rss_dist = + calcRssDistance(0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params); // It is difficult to define the detection range. If it is too short, the stuck will not be // determined, even though you are stuck by an obstacle. If it is too long, // the ego will be judged to be stuck by a distant vehicle, even though the ego is only // stopped at a traffic light. Essentially, the calculation should be based on the information of // the stop reason, but this is outside the scope of one module. I keep it as a TODO. - constexpr double DETECTION_DISTANCE_MARGIN = 10.0; - const auto detection_distance = current_max_dist_buffer + rss_dist + - getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; - RCLCPP_DEBUG( - logger_, "current_max_dist_buffer: %f, max_acc: %f", current_max_dist_buffer, max_acc); + constexpr auto detection_distance_margin = 10.0; + const auto obstacle_check_distance = common_data_ptr_->transient_data.lane_changing_length.max + + rss_dist + common_data_ptr_->bpp_param_ptr->base_link2front + + detection_distance_margin; + const auto has_object_blocking = std::any_of( + filtered_objects_.current_lane.begin(), filtered_objects_.current_lane.end(), + [&](const auto & object) { + // Note: it needs chattering prevention. + if ( + std::abs(object.initial_twist.linear.x) > + lc_param_ptr->stopped_object_velocity_threshold) { // check if stationary + return false; + } - auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); + const auto ego_to_obj_dist = + calcSignedArcLength( + current_lanes_path.points, ego_pose.position, object.initial_pose.position) - + obstacle_check_distance; + return ego_to_obj_dist < 0.0; + }); - lane_change_debug_.is_stuck = is_vehicle_stuck; - return is_vehicle_stuck; + lane_change_debug_.is_stuck = has_object_blocking; + return has_object_blocking; } bool NormalLaneChange::is_valid_start_point( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 8794d79a4d10f..20bc982445b07 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -78,6 +78,12 @@ rclcpp::Logger get_logger() return logger; } +bool is_mandatory_lane_change(const ModuleType lc_type) +{ + return lc_type == LaneChangeModuleType::NORMAL || + lc_type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE; +} + double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity) { @@ -160,14 +166,13 @@ lanelet::ConstLanelets getTargetNeighborLanes( lanelet::ConstLanelets neighbor_lanes; for (const auto & current_lane : current_lanes) { + const auto mandatory_lane_change = is_mandatory_lane_change(type); if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { - if ( - type == LaneChangeModuleType::NORMAL || - type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { + if (mandatory_lane_change) { neighbor_lanes.push_back(current_lane); } } else { - if (type != LaneChangeModuleType::NORMAL) { + if (!mandatory_lane_change) { neighbor_lanes.push_back(current_lane); } } @@ -322,12 +327,17 @@ std::optional construct_candidate_path( return std::optional{candidate_path}; } -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, const double next_lane_change_buffer) +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval) { + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; + const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; + const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; + const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; + const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -337,10 +347,10 @@ PathWithLaneId getReferencePathFromTargetLane( if (is_goal_in_route) { const double s_goal = lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - - next_lane_change_buffer; + next_lc_buffer; return std::min(dist_from_lc_start, s_goal); } - return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); + return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); }); constexpr double epsilon = 1e-4; @@ -548,10 +558,13 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr) { + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto rough_shift_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose).distance; @@ -625,17 +638,16 @@ CandidateOutput assignToCandidate( return candidate_output; } -std::optional getLaneChangeTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType type, const Direction & direction) +std::optional get_lane_change_target_lane( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes) { - if ( - type == LaneChangeModuleType::NORMAL || - type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { - return route_handler.getLaneChangeTarget(current_lanes, direction); + const auto direction = common_data_ptr->direction; + const auto route_handler_ptr = common_data_ptr->route_handler_ptr; + if (is_mandatory_lane_change(common_data_ptr->lc_type)) { + return route_handler_ptr->getLaneChangeTarget(current_lanes, direction); } - return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); + return route_handler_ptr->getLaneChangeTargetExceptPreferredLane(current_lanes, direction); } std::vector convertToPredictedPath( @@ -817,8 +829,7 @@ bool isParkedObject( bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug) + const std::vector & objects, CollisionCheckDebugMap & object_debug) { const auto route_handler = *common_data_ptr->route_handler_ptr; const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; @@ -868,7 +879,7 @@ bool passed_parked_objects( }); // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane <= minimum_lane_change_length) { + if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) { return true; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index f73f989174b54..3d7532ba5b244 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -113,7 +113,7 @@ PredictedObjects filterObjects( */ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, const double velocity_threshold, - const bool remove_above_threshold = true); + const bool remove_above_threshold = false); /** * @brief Helper function to filter objects based on their velocity. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 7ac9993ee8b01..2e13c75a46481 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -104,6 +104,33 @@ FrenetPoint convertToFrenetPoint( return frenet_point; } +/** + * @brief Converts a Lanelet point to a ROS Pose message. + * + * This function converts a point from a Lanelet map to a ROS geometry_msgs::msg::Pose. + * It sets the position from the point and calculates the orientation (yaw) based on the target + * lane. + * + * @tparam LaneletPointType The type of the input point. + * + * @param[in] src_point The point to convert. + * @param[in] target_lane The lanelet used to determine the orientation. + * + * @return A Pose message with the position and orientation of the point. + */ +template +Pose to_geom_msg_pose(const LaneletPointType & src_point, const lanelet::ConstLanelet & target_lane) +{ + const auto point = lanelet::utils::conversion::toGeomMsgPt(src_point); + const auto yaw = lanelet::utils::getLaneletAngle(target_lane, point); + geometry_msgs::msg::Pose pose; + pose.position = point; + tf2::Quaternion quat; + quat.setRPY(0, 0, yaw); + pose.orientation = tf2::toMsg(quat); + return pose; +} + // distance (arclength) calculation double l2Norm(const Vector3 vector); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 25b307ab2cc4d..20221fff82175 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -118,7 +118,7 @@ PredictedObjects filterObjects( const ObjectTypesToCheck & target_object_types = params->object_types_to_check; PredictedObjects filtered_objects = - filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false); + filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, true); filterObjectsByClass(filtered_objects, target_object_types); @@ -136,7 +136,7 @@ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, const double velocity_threshold, const bool remove_above_threshold) { - if (remove_above_threshold) { + if (!remove_above_threshold) { return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold); } return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::max()); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index a48e36e3599ac..f3f6df142185b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -896,25 +896,8 @@ double getArcLengthToTargetLanelet( const auto target_center_line = target_lane.centerline().basicLineString(); - Pose front_pose, back_pose; - - { - const auto front_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.front()); - const double front_yaw = lanelet::utils::getLaneletAngle(target_lane, front_point); - front_pose.position = front_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, front_yaw); - front_pose.orientation = tf2::toMsg(tf_quat); - } - - { - const auto back_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.back()); - const double back_yaw = lanelet::utils::getLaneletAngle(target_lane, back_point); - back_pose.position = back_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, back_yaw); - back_pose.orientation = tf2::toMsg(tf_quat); - } + const auto front_pose = to_geom_msg_pose(target_center_line.front(), target_lane); + const auto back_pose = to_geom_msg_pose(target_center_line.back(), target_lane); const auto arc_front = lanelet::utils::getArcCoordinates(lanelet_sequence, front_pose); const auto arc_back = lanelet::utils::getArcCoordinates(lanelet_sequence, back_pose); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 9df0791ed9375..f44bf204bd2d7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -329,7 +329,7 @@ bool StartPlannerModule::noMovingObjectsAround() const utils::path_safety_checker::filterObjectsByClass( dynamic_objects, parameters_->surround_moving_obstacles_type_to_check); const auto filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( - dynamic_objects, parameters_->th_moving_obstacle_velocity, false); + dynamic_objects, parameters_->th_moving_obstacle_velocity, true); if (!filtered_objects.objects.empty()) { DEBUG_PRINT("Moving objects exist in the safety check area"); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 1ae99cafaceb1..609ff010d20e0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -441,6 +441,7 @@ void IntersectionModuleManager::setActivation() scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); intersection_module->setOcclusionActivation( occlusion_rtc_interface_.isActivated(occlusion_uuid)); + scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(getUUID(scene_module->getModuleId()))); } } diff --git a/sensing/cuda_utils/package.xml b/sensing/cuda_utils/package.xml index 4ae44469f1efa..b7690bd9e8cc8 100644 --- a/sensing/cuda_utils/package.xml +++ b/sensing/cuda_utils/package.xml @@ -5,8 +5,9 @@ cuda utility library Daisuke Nishimatsu - Daisuke Nishimatsu Manato Hirabayashi + Kenzo Lobos-Tsunekawa + Amadeusz Szymko Apache License 2.0